#include "coeecatdevice.h"

//#include <rtdk.h>
//#include <rtdm/rtdm.h>
//#include <pthread.h>

#include <unistd.h>
#include <iostream>
#include <stdio.h>
#include <assert.h>

#include <QCoreApplication>

#include "powermanager.h"
#include "robotmanager.h"
#include "messagelog.h"

#include "timetool.h"
//#include <lib/master.h>


#if D_WRITE_SERVO_DATA_TO_FILE
#include <stdio.h>
FILE *fpServoDataFile=NULL;
bool fpFlush=false;
unsigned int servo_data_id=0;
#endif


class PowerManager;
extern PowerManager *p_powerManger;

#pragma pack(push) //保存对齐状态
#pragma pack(1)//设定为1字节对齐

/// 2016.11.10.xyf.经测试 //io必须使用EC_WD_ENABLE才能写 DO
/// 对于8bit数据 三洋电机必须用   {0x0000, 0x00, 8}, // 假字节 补为16bit
/// 清能德创  用  {0x5FFE, 0x00, 8}, // 假字节 补全

/**
 * =========== default =============================
 */
ec_pdo_entry_info_t motor_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    {0x6060, 0x00, 8}, //控制模式
    //    {0x0000, 0x00, 8}, // 假字节 (三洋写了假字节后报错)

    //    {0x60FF, 0x00, 32}, //目标速度
    //    {0x6071, 0x00, 16}, //目标力距
    //    {0x6081, 0x00, 32}, //目标速度
    //    {0x6098, 0x00, 8}, //回零模式
    //    {0x6099, 0x01, 32},//回零速度 01寻找回零开关速度, 02寻找Z相速度
    //    {0x6099, 0x02, 32},//回零速度 01寻找回零开关速度, 02寻找Z相速度
    //    {0x60B1, 0x00, 32},// 速度漂移
    //    {0x60B2, 0x00, 16},//扭矩漂移
    //    {0x609A, 0x00, 32}, //LS err: not PDO

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值)
  //  {0x6061, 0x00, 8}, //控制模式显示
    //   {0x0000, 0x00, 8}, // 假字节
  //  {0x606C, 0x00, 32}, //实际速度
  //  {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
 //   {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x603F, 0x00, 16},//错误码
};
// {0x6065, 0x00, 32} // 设定电机跟随误差( LS no such pdo )

/// 不同品牌的 PDO 映射的通道 index 不一样, 一个通道对象字典可以超过12个
//LS 不能用0x1700,　0x1b00
ec_pdo_info_t motor_pdos[] = {
    {0x1600, 3, motor_entries + 0},
    {0x1a00, 3, motor_entries + 3}
};

ec_sync_info_t motor_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, motor_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, motor_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/**
 * =========== 同川 tcr=============================
 */
ec_pdo_entry_info_t tcr_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    {0x6060, 0x00, 8}, //控制模式

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值)
   // {0x6061, 0x00, 8}, //控制模式显示
    {0x606C, 0x00, 32}, //实际速度
    {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
//    {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x603F, 0x00, 16},//错误码
};

ec_pdo_info_t tcr_pdos[] = {
    {0x1600, 3, tcr_entries + 0},
    {0x1a00, 5, tcr_entries + 3}
};

ec_sync_info_t tcr_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, tcr_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, tcr_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/**
 * =========== 松下 =============================
 */
ec_pdo_entry_info_t sx_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    {0x6060, 0x00, 8}, //控制模式

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值)
   // {0x6061, 0x00, 8}, //控制模式显示
    {0x606C, 0x00, 32}, //实际速度
    {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
  //  {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x603F, 0x00, 16},//错误码
};

ec_pdo_info_t sx_pdos[] = {
    {0x1600, 3, sx_entries + 0},
    {0x1a00, 5, sx_entries + 3}
};

ec_sync_info_t sx_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, sx_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, sx_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/**
 * =========== LS =============================
 */
ec_pdo_entry_info_t ls_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    {0x6060, 0x00, 8}, //控制模式

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值)
 //   {0x6061, 0x00, 8}, //控制模式显示
    {0x606C, 0x00, 32}, //实际速度
    {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
  //  {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x603F, 0x00, 16},//错误码
};

ec_pdo_info_t ls_pdos[] = {
    {0x1600, 3, ls_entries + 0},
    {0x1a00, 5, ls_entries + 3}
};

ec_sync_info_t ls_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, ls_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, ls_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/**
 * =========== 台达 =============================
 */
ec_pdo_entry_info_t delta_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    {0x6060, 0x00, 8}, //控制模式

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值)
    {0x6061, 0x00, 8}, //控制模式显示
    {0x606C, 0x00, 32}, //实际速度
    {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
    {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x603F, 0x00, 16},//错误码
};

ec_pdo_info_t delta_pdos[] = {
    {0x1600, 3, delta_entries + 0},
    {0x1a00, 7, delta_entries + 3}
};

ec_sync_info_t delta_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, delta_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, delta_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/**
 * =========== Invt =============================
 */
ec_pdo_entry_info_t invt_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    {0x6060, 0x00, 8}, //控制模式

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值)
    {0x6061, 0x00, 8}, //控制模式显示
    {0x606C, 0x00, 32}, //实际速度
    {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
    {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x603F, 0x00, 16},//错误码
};

ec_pdo_info_t invt_pdos[] = {
    {0x1600, 3, invt_entries + 0},
    {0x1a00, 7, invt_entries + 3}
};

ec_sync_info_t invt_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, invt_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, invt_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/**
 * =========== 三洋 =============================
 */
ec_pdo_entry_info_t sanyo_entries[] = {
    //write
    {0x6040, 0x00, 16}, //控制字( 这个必须写在开头 )
    {0x607A, 0x00, 32}, //目标位置
    //     {0x6060, 0x00, 8}, //控制模式 (三洋写了假字节后报错,应该是主站的问题)
    //    {0x0000, 0x00, 8}, // 假字节

    //   {0x60FF, 0x00, 32}, //目标速度 invt
    //     {0x6071, 0x00, 16}, //目标力距
    //      {0x6081, 0x00, 32}, //目标速度
    //    {0x6098, 0x00, 8}, //回零模式
    //                          {0x6099, 0x01, 32},//回零速度 01寻找回零开关速度, 02寻找Z相速度
    //                          {0x6099, 0x02, 32},//回零速度 01寻找回零开关速度, 02寻找Z相速度
    //                        {0x60B1, 0x00, 32},// 速度漂移
    //                        {0x60B2, 0x00, 16},//扭矩漂移
    // {0x609A, 0x00, 32}, //LS err: not PDO

    // read
    {0x6041, 0x00, 16},//状态字
    {0x6064, 0x00, 32},//实际位置(编码器值) (同川伺服，0x1a01只能映射0x6041和0x6064两个)
    //   {0x1001, 0x00, 8},//错误码
    //    {0x6061, 0x00, 8}, //控制模式显示
    //  {0x0000, 0x00, 8}, // 假字节 (三洋好像只能放在最後面)
    {0x606C, 0x00, 32}, //实际速度
    {0x6077, 0x00, 16}, //实际扭矩
    //   {0x60FD, 0x00, 32},//数字输入
  //  {0x60F4, 0x00, 32}, // 实际跟随误差
    {0x2102, 0x01, 32},//错误码( 读后8bit )
};

ec_pdo_info_t sanyo_pdos[] = {
    {0x1700, 2, sanyo_entries + 0},
    {0x1b00, 5, sanyo_entries + 2}
};

ec_sync_info_t sanyo_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, sanyo_pdos + 0, EC_WD_DISABLE},
    {3, EC_DIR_INPUT, 1, sanyo_pdos + 1, EC_WD_DISABLE},
    {0xff}
};


#pragma pack()//默认的对齐



/****************************************************************************/
// EtherCAT distributed clock variables

#define DC_FILTER_CNT          1024
#define SYNC_MASTER_TO_REF        1

static uint64_t dc_start_time_ns = 0LL;
static uint64_t dc_time_ns = 0;
#if SYNC_MASTER_TO_REF
static uint8_t  dc_started = 0;
static int32_t  dc_diff_ns = 0; //主站 跟 参考时钟的 时间差
static int32_t  prev_dc_diff_ns = 0;
static int64_t  dc_diff_total_ns = 0LL;
static int64_t  dc_delta_total_ns = 0LL;
static int      dc_filter_idx = 0;
static int64_t  dc_adjust_ns;
#endif
static int64_t  system_time_base = 0LL; //调整后的主站时钟？
//static uint64_t wakeup_time = 0LL;
//static uint64_t overruns = 0LL;


quint64  get_ns_to_2000y()
{
    struct timespec getTime ;
    clock_gettime(CLOCK_MONOTONIC, &getTime);

    // return  (((getTime).tv_sec) * 1000000000ULL + (getTime).tv_nsec);
    return EC_TIMEVAL2TCNANO( getTime );
}
quint64  get_ns_to_2000y( struct timespec &time )
{

    // return  (((time).tv_sec) * 1000000000ULL + (time).tv_nsec);
    return EC_TIMEVAL2TCNANO( time );
}

struct timespec  ns_2000y_to_now_time(  quint64 ns )
{
    //      (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec)
    struct timespec newTime ;
    newTime.tv_sec=   ns /1000000000ULL + 946684800ULL ;
    newTime.tv_nsec=   ns %1000000000ULL;
    return  newTime;
}

static void  sleep_ns_real_time( int sleep_ns )
{
    struct timespec sleep_time;
    clock_gettime(CLOCK_MONOTONIC , &sleep_time);//
    sleep_time.tv_nsec += sleep_ns ;
    while (sleep_time.tv_nsec >= NSEC_PER_SEC)
    {
        sleep_time.tv_nsec -= NSEC_PER_SEC;
        sleep_time.tv_sec++;
    }
    clock_nanosleep(CLOCK_MONOTONIC , TIMER_ABSTIME, &sleep_time, NULL);
}

#if D_IF_DEBUG
int min_v[3]={-10000,-20000,-50000};
int max_v[3]={10000,20000,50000};
int timer_id[20];
enum E_TIME{
    D_TIME_CYCLE,
    D_TIME_WORK,
    D_TIME_SLEEP,
    D_TIME_MATH,
    D_TIME_RW,
    D_TIME_SEND,
    D_TIME_READ,
    D_TIME_TIMER,

    D_TIME_MAX
};
#endif


CoeEcatDevice::CoeEcatDevice()
{
    setComponentVersion( "CoeEcatDevice", "2017-5-5", "1.1.0" );

    //ethercat 线程锁
    pthread_mutexattr_init(&ecatAttr);
    pthread_mutexattr_setprotocol(&ecatAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&ecatThreadMutex,&ecatAttr);

}

CoeEcatDevice::~CoeEcatDevice()
{

#ifdef SIMULATION_MODE
    if(  tcpVrep!=NULL )
    {
        tcpVrep->close();
    }
#endif
    if( master  )
    {
        rt_printf( "release master...\n" );

//        if(isMasterActived)
//            ecrt_master_deactivate(master);

        ecrt_release_master(master);
        master=NULL;
    }
    if(escDevice)
    {
        delete escDevice;
        escDevice=NULL;
    }
}

CoeEcatDevice* CoeEcatDevice::escDevice=NULL;
CoeEcatDevice* CoeEcatDevice::getInstance()
{
    if(NULL == escDevice)
    {
        escDevice = new CoeEcatDevice();
    }
    return escDevice;
}

int CoeEcatDevice::state()
{
    if(isRequireReboot)
        return EC_FAILED;
    else if( error_flag )
        return EC_ERR;
    else if(isInitOk && readdyOk)
        return EC_OP;
    else if( isInitOk )
        return EC_INIT;
    else
        return EC_PREOP;
}

bool CoeEcatDevice::init( int sleeptime_us  )
{
#if D_WRITE_SERVO_DATA_TO_FILE

    qDebug()<<"开启:将算法的数据写入到文件功能!";
    servo_data_id=0;
    FILE* fd = fopen( D_TMP_SERVO_DATA_FILE ,"w");
    if(fd!=NULL){
        fclose(fd);
    }

    if ((fpServoDataFile=fopen(D_TMP_SERVO_DATA_FILE,"a+"))==NULL)//
    {
        qDebug()<< "文件打开失败:" <<D_TMP_SERVO_DATA_FILE;
    }
#endif
    cycle_ns = sleeptime_us*1000;
    if( cycle_ns<1000 ){
        cycle_ns=1000000;
    }

#if D_IF_DEBUG
    int pttime=5000;//5000=5s
    timer_id[D_TIME_CYCLE]=TimeTool::getInstance()->creatTimer( "cycle", cycle_ns, pttime,min_v, max_v , true );
    timer_id[D_TIME_WORK]=TimeTool::getInstance()->creatTimer( "work",   cycle_ns, pttime,min_v, max_v , true);
    timer_id[D_TIME_SLEEP]=TimeTool::getInstance()->creatTimer( "sleep", cycle_ns, pttime,min_v, max_v , true);
    timer_id[D_TIME_MATH]=TimeTool::getInstance()->creatTimer( "algorithm", cycle_ns, pttime,min_v, max_v, true );
    timer_id[D_TIME_RW]=TimeTool::getInstance()->creatTimer( "rw",       cycle_ns, pttime,min_v, max_v , true);
    timer_id[D_TIME_SEND]=TimeTool::getInstance()->creatTimer( "send",   cycle_ns, pttime,min_v, max_v, true );
    timer_id[D_TIME_READ]=TimeTool::getInstance()->creatTimer( "read",   cycle_ns, pttime,min_v, max_v , true);
    timer_id[D_TIME_TIMER]=TimeTool::getInstance()->creatTimer( "timer",   cycle_ns, pttime,min_v, max_v , true);

    for(int i=0;i<10;i++){
        qDebug()<<"timer_id= ============="<< timer_id[i];
    }
#endif

    t1={0,0};
    t2={0,0};
    t3={0,0};
    t4={0,0};

    master=NULL;        //主站master
    domain=NULL;       //一个结构体域的指针
    domain_p=NULL;        //域数据的指针
    busXml=NULL;
    sc_ref = NULL;

    domainOffKeep=0;        //域的偏移记录   bit
    offCntKeep=0;       //偏移数记录    偏移的对象字典

    isMasterActived = false;
    isInitOk = false;
    checkTime = false;
    isRequireReboot=false;

    error_flag_count=3;
    error_flag = 0;
    sdoError = 0;   //sdo的错误
    aliasMatchError = 0;  //别名匹配错误
    etherCatCommError = 0;//通信错误
    slaveConfigError = 0; //从站配置失败
    domainStateRrror = 0;//域数据读写失败，

    warningState = false;
    faultOccurred = false;

    check_error_delay=5000;
    cycle_counter=0;     //循环计数
    sync_ref_counter=0;      //同步参考计数
    readdyOk=false;
    //  servoSwitch=EM_CF_OFF;//使能开关
    resetRequestCnt=0;// 清零标志
    //  servoStatus=EM_DRIVER_UNSTANDBY;

    sdoError=0;//sdo初始化错误
    aliasMatchError=0;//别名不匹配
    etherCatCommError=0;//通信错误
    slaveConfigError=0;//初始化配置失败
    domainStateRrror=0;//域数据读写失败

    // memset( &master_state, 0, sizeof(ec_master_state_t) );
    memset( &domain_state, 0, sizeof(ec_domain_state_t) );
    master_state.al_states=0;
    master_state.link_up=0;

    entriesCount=0;
    motorCount=0;
    doCount=0;
    diCount=0;
    aoCount=0;
    aiCount=0;
    //ioCounter=0;//
    get_data_false_count=0;

    ioDataOffsetList.clear();
    ioDeviceDataOffsetMap.clear();

    ///函数不可调换顺序

    int devcount=7;
#ifdef SIMULATION_MODE
    //检测Ethercat 总线上的所有设备
    pointProInterval=0;
    pointProInterval_counter = 0;
    tcpCounter=0;

    ecatDevs.resize(devcount);
    aliasMapNode.clear();
    for(int i=0; i<devcount; i++)
    {
        ecatDevs[i].devInfo.node = i;
        ecatDevs[i].devInfo.alias=81+i;

        if( i==0  )
        {
            ecatDevs[i].devInfo.vendorId = 0x66f;
            ecatDevs[i].devInfo.productCode = 0x525100a1;
            ecatDevs[i].devInfo.revisionNumber= 0x00010000;
            ecatDevs[i].devInfo.devName = "虚拟 电机１";
            ecatDevs[i].devInfo.devType = E_EcatDev_Servo ;
        }
        else     if( i==1  )
        {

            ecatDevs[i].devInfo.vendorId = 0x66f;
            ecatDevs[i].devInfo.productCode = 0x515070a1;
            ecatDevs[i].devInfo.revisionNumber= 0x00010000;
            ecatDevs[i].devInfo.devName = "虚拟 电机２";
            ecatDevs[i].devInfo.devType = E_EcatDev_Servo ;
        }else     if( i==2  )
        {

            ecatDevs[i].devInfo.vendorId = 0x66f;
            ecatDevs[i].devInfo.productCode = 0x515070a1;
            ecatDevs[i].devInfo.revisionNumber= 0x00010000;
            ecatDevs[i].devInfo.devName = "虚拟 电机３";
            ecatDevs[i].devInfo.devType = E_EcatDev_Servo ;
        }else     if( i==3  )
        {
            ecatDevs[i].devInfo.vendorId = 0x66f;
            ecatDevs[i].devInfo.productCode = 0x515050a1;
            ecatDevs[i].devInfo.revisionNumber= 0x00010000;
            ecatDevs[i].devInfo.devName = "虚拟 电机4 ";
            ecatDevs[i].devInfo.devType = E_EcatDev_Servo ;
#ifdef D_VREP_ROBOT_AXIS_6
        }else     if( i==4  )
        {
            ecatDevs[i].devInfo.vendorId = 0x66f;
            ecatDevs[i].devInfo.productCode = 0x515050a1;
            ecatDevs[i].devInfo.revisionNumber= 0x00010000;
            ecatDevs[i].devInfo.devName = "虚拟 电机5 ";
            ecatDevs[i].devInfo.devType = E_EcatDev_Servo ;
        }else     if( i==5  )
        {
            ecatDevs[i].devInfo.vendorId = 0x66f;
            ecatDevs[i].devInfo.productCode = 0x515050a1;
            ecatDevs[i].devInfo.revisionNumber= 0x00010000;
            ecatDevs[i].devInfo.devName = "虚拟 电机6 ";
            ecatDevs[i].devInfo.devType = E_EcatDev_Servo ;
#else
        }else     if( i==4  )
        {

            ecatDevs[i].devInfo.vendorId = 0x02;
            ecatDevs[i].devInfo.productCode = 0x044c2c52;
            ecatDevs[i].devInfo.revisionNumber= 0x00120000;
            ecatDevs[i].devInfo.devName = "虚拟 耦合器数据 ";
            ecatDevs[i].devInfo.devType = E_EcatDev_Coupler ;
        }else     if( i==5  )
        {

            ecatDevs[i].devInfo.vendorId = 0x02;
            ecatDevs[i].devInfo.productCode = 0x0b493052;
            ecatDevs[i].devInfo.revisionNumber= 0x00110000;
            ecatDevs[i].devInfo.devName = "虚拟 IO设备1";
            ecatDevs[i].devInfo.devType = E_EcatDev_IoModule ;
#endif
        }else     if( i==6  )
        {

            ecatDevs[i].devInfo.vendorId = 0x02;
            ecatDevs[i].devInfo.productCode = 0x07613052;
            ecatDevs[i].devInfo.revisionNumber= 0x00120000;
            ecatDevs[i].devInfo.devName = "虚拟 IO设备2";
            ecatDevs[i].devInfo.devType = E_EcatDev_IoModule ;
        }

        ecatDevs[i].devInfo.devType=E_EcatDev_New;
        ecatDevs[i].devInfo.mappingObjectFile="";

        qDebug()<<"xxx=======xxxxxxxxxxxxecatDevs[i].devInfo.alias="<<ecatDevs[i].devInfo.alias;
        aliasMapNode.insertMulti(ecatDevs[i].devInfo.alias,i);
        nodeMapAlias.insert(i,ecatDevs[i].devInfo.alias);
    }

    //读取总线配置文件
    if(busXml==NULL){
        busXml=new EcatBusXml(D_ECATBUS_CONFIG_FILE);
    }

    if(!busXml->readBusConfig())
        return false;

    ioDevAlias.clear();
    robotUseAliasList.clear();
    devcount=busXml->aliasList.size();
    for(int i=0;i<devcount;i++)
    {
        int tAlias=busXml->aliasList.at(i);
        int type= busXml->aliasMapDeviceType.value( tAlias, E_EcatDev_TypeErr );
        if( type == E_EcatDev_Coupler||  type ==E_EcatDev_IoModule)
        {
            ioDevAlias.append( tAlias );
            robotUseAliasList.append( tAlias );
        }
    }

#else
    if( detectionEcatBusDevs() )
    {
        rt_printf("Detection EcatBus devs ................OK!\n");
    }else{
        rt_printf("Detection EcatBus devs ................FAIL!\n");
        goto LAB_END;
    }
#endif

    //读取机器人配置信息
    if( readRobotsConfig() )
    {
        rt_printf("Read robots config ....................OK!\n");
    }else{
        rt_printf("Read robots config ....................FAIL!\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 10, -1 );
        goto LAB_END;
    }

    //获取已使用设备的总线设备配置信息
    if( readEcatBusDevsConfig() )
    {
        rt_printf("Read EcatBus devs config ..............OK!\n");
    }else{
        rt_printf("Read EcatBus devs config .............FAIL!\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 11, -1 );
        goto LAB_END;
    }

    resizeAllVector();   //设置容器大小
    displayBusDevicesInfo();  //打印一些信息

#ifndef SIMULATION_MODE
    master = ecrt_request_master(0);
    if( master )
    {
        rt_printf("Requesting master ....................OK!\n");
    }else{
        rt_printf("Requesting master ....................FAIL!\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 12, -1 );
        goto LAB_END;
    }

    domain = ecrt_master_create_domain( master );
    if( domain )
    {
        rt_printf("Registering domain ...................OK!\n");
    }else{
        rt_printf("Registering domain ...................FAIL!\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 13, -1 );
        goto LAB_END;
    }

    //别名检测
    if( checkAlias() )
    {
        rt_printf("Check alias ..........................OK!\n");
    }else{
        rt_printf("Check alias ..........................FAIL! %d \n", aliasMatchError);
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 14, -1 );
        goto LAB_END;
    }

    //初始化设备
    if( initDevice() )
    {
        isInitOk=true;
     //   qDebug()<< "Init device ..........................OK!";
         rt_printf("Init device ..........................OK!\n");
    }else{
       // qDebug()<< "Init device ..........................FAIL!";
        rt_printf("Init device ..........................FAIL!\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 15, -1 );
        goto LAB_END;
    }
  //  qDebug( "352h ai=%d ao=%d di=%d do=%d  \n", aiCount, aoCount ,diCount ,doCount );
   rt_printf( "352h ai=%d ao=%d di=%d do=%d  \n", aiCount, aoCount ,diCount ,doCount );

    servoOffRobot(-1);//先关掉所有伺服,条件：  isInitOk=true;
    setAmpModeRobot(-1, E_ServoModeCsp);//模式 E_ServoModePp

    // displayDomainRegConfig();
    //        displayEthercatConfig(0);
    //        displayEthercatConfig(1);
    //        displayEthercatConfig(2);
    //        displayEthercatConfig(3);
    //    displayEthercatConfig(4);
    //    displayEthercatConfig(5);
    //    displayEthercatConfig(6);


#else
    ioDataOffsetList.resize( devcount );
    v_homeData.resize( devcount );
    v_encoder_t.resize( 0 );
    if( devcount==7 )
    {
#ifdef D_VREP_ROBOT_AXIS_6
        ioDevAlias.resize(1);
        ioDevAlias[0] =  87;
        ioDeviceData.resize( 1 );

        //DI  //DO
        ioDeviceData[0].ioAo.resize( 0 );
        ioDeviceData[0].ioAi.resize( 0 );
        ioDeviceData[0].ioDi.resize( 16 );
        ioDeviceData[0].ioDo.resize( 0 );
        ioDeviceData[0].nodeId  = ecatDevs[6].devInfo.alias;

        ioDeviceDataMap.insert( 87, &ioDeviceData[0] );
        ioDeviceDataOffsetMap.insert(  87, &(ioDataOffsetList[6]) );
#else
        ioDevAlias.resize(3);
        ioDevAlias[0] =  87 ;
        ioDevAlias[1] = 86 ;
        ioDevAlias[2]=  87 ;

        ioDeviceData.resize( 3 );

        //耦合器
        ioDeviceData[0].ioAo.resize( 0  );
        ioDeviceData[0].ioAi.resize(  0 );
        ioDeviceData[0].ioDi.resize( 0  );
        ioDeviceData[0].ioDo.resize( 0 );
        ioDeviceData[0].nodeId  = ecatDevs[4].devInfo.alias;

        //DO
        ioDeviceData[1].ioAo.resize( 0  );
        ioDeviceData[1].ioAi.resize(  0 );
        ioDeviceData[1].ioDi.resize( 0  );
        ioDeviceData[1].ioDo.resize( 16 );
        ioDeviceData[1].nodeId  = ecatDevs[5].devInfo.alias;

        //DI
        ioDeviceData[2].ioAo.resize(  0  );
        ioDeviceData[2].ioAi.resize( 0 );
        ioDeviceData[2].ioDi.resize( 16 );
        ioDeviceData[2].ioDo.resize(  0  );
        ioDeviceData[2].nodeId  = ecatDevs[6].devInfo.alias;

        ioDeviceDataMap.insert( ioDeviceData[0].nodeId, &ioDeviceData[0] );
        ioDeviceDataMap.insert( ioDeviceData[1].nodeId, &ioDeviceData[1] );
        ioDeviceDataMap.insert( ioDeviceData[2].nodeId, &ioDeviceData[2] );

        ioDeviceDataOffsetMap.insert(  ecatDevs[5].devInfo.alias, &(ioDataOffsetList[5]) );
        ioDeviceDataOffsetMap.insert(  ecatDevs[6].devInfo.alias, &(ioDataOffsetList[6]) );
#endif

    }

    tcpVrep=NULL;
    isInitOk=true;
    readdyOk=true;

#endif


LAB_END:

    if( !isInitOk )
    {
        //初始化失败后，配置一些数据，防止访问越界导致程序崩溃
        servoStateData.resize(32);
        ioDeviceData.resize(0);
        nodeToRobotIdAndAxit.resize(32);

        RobotInfo tRob;
        foreach(tRob,robots)
        {
            QVector<ServoState*>  pData;
            pData.resize( tRob.motorAlias.size() );
            for(int i=0; i<pData.size(); i++ )
            {
                pData[i] = &(servoStateData[i]);
                nodeToRobotIdAndAxit[i].axis=i;
                nodeToRobotIdAndAxit[i].robotId=tRob.robotId;
            }
            servoStateMap.insert(  tRob.robotId, pData  );
            robotServoOnFlag.insert( tRob.robotId, false  );
        }
    }

    for( int j=0;j<ioDeviceData.size();j++ )
    {
        ioDeviceDataMap.insert( ioDeviceData.at(j).nodeId , &(ioDeviceData[j]) );
    }

//#ifndef SIMULATION_MODE
//    if( isMasterActived ){
//        waitNodeReadyOk();
//    }
//#endif

 //   cycleStartTime();//设置睡眠时间
    return isInitOk;
}
//end init()


void CoeEcatDevice::resizeAllVector()
{
    for(int i = 0; i < robotUseAliasList.size(); i++ )
    {
        S_EcatDevInfo* robDev=robotDevs.value( robotUseAliasList.at(i) ,NULL);
        if(robDev==NULL )
        {
            qDebug()<<"robDev NULL Err:"<<__LINE__;
            return;
        }
        entriesCount += robDev->objConfig.ec_entries.size();

        if(robDev->devInfo.devType==E_EcatDev_IoModule)
        {
            /*
        for(int j=0;j<robDev->objConfig.ec_syncs.size();j++)
            {
                if( robDev->objConfig.ec_syncs.at(j).dir == EC_DIR_OUTPUT )
                {
                    for(int k=0;k<robDev->objConfig.ec_syncs.at(j).n_pdos;k++ )
            {
                        doCount+=robDev->objConfig.ec_syncs.at(j).pdos[k].n_entries;
                    }
                }
                else if( robDev->objConfig.ec_syncs.at(j).dir == EC_DIR_INPUT )
                {
                    for(int k=0;k<robDev->objConfig.ec_syncs.at(j).n_pdos;k++ )
            {
                        diCount+=robDev->objConfig.ec_syncs.at(j).pdos[k].n_entries;
                    }
                }
            }*/
        }else if( robDev->devInfo.devType==E_EcatDev_Servo )
        {
            motorCount ++ ;
        }
    }

    qDebug()<<"xxx numbers:"
           <<motorCount
           << entriesCount
           << ecatDevs.size()
           << robotDevs.size()
           <<ioDevAlias.size();

    builddomainRegs.resize( entriesCount+1 );//注册每个对象字典信息//must be lager thean entriesCount
    ioDataOffsetList.resize( ecatDevs.size() );
    homeSpeedToSwitch.resize( ecatDevs.size() );       //寻找回零开关的速度
    homeSpeedToZ.resize( ecatDevs.size() );     //寻找Z相的速度
    homeMethod.resize( ecatDevs.size() );        //回零方式
    homingAcc.resize( ecatDevs.size() );    //回零加速度
    followingErrorActualValue.resize( ecatDevs.size() );//跟随误差
    vGohomeFlag.resize( ecatDevs.size() );

    startHomeTime.resize( ecatDevs.size() );
    endHomeTime.resize( ecatDevs.size() );
    diffHomeTime.resize( ecatDevs.size() );      //回零时间超时
    encoderFactor.resize( ecatDevs.size() );      //编码因子
    updateHomeOffset.resize( ecatDevs.size() );//

    ctlWord.resize( ecatDevs.size() );        //控制字
    staWord.resize( ecatDevs.size() );        //状态字
    targetPos.resize( ecatDevs.size() );     //目的地址
    encoderValue.resize( ecatDevs.size() );      //编码器的值
    encoderValueOld.resize( ecatDevs.size() );   //旧的编码器的值
    realEncoderValue.resize( ecatDevs.size() );
    /*   v.resize( ecatDevs.size() );      //速度
    vOld.resize( ecatDevs.size() );       //上一个周期的速度
    a.resize( ecatDevs.size() );     //加速度
    aOld.resize( ecatDevs.size() );       //上一个周期的加速度
 //   jerk.resize( ecatDevs.size() );       //加加速
*/
    followingErrorData.resize( ecatDevs.size() );//跟随误差
    errorCode.resize( ecatDevs.size() );     //错误码
    ampMode.resize( ecatDevs.size() );    //驱动器的模式
    torqueOffset.resize( ecatDevs.size() );    //扭矩偏移
    targetTorque.resize( ecatDevs.size() );
    torqueActValue.resize( ecatDevs.size() );      //扭矩实际值
    velocityActValue.resize( ecatDevs.size() );   //速度实际值
    maxSpeed.resize( ecatDevs.size() );   //最大速度
    targetSpeed.resize( ecatDevs.size() );   //目标速度
    motorDi.resize( ecatDevs.size() );      //驱动器上的IO输入

    ecOffOd.resize( ecatDevs.size() );//伺服电机模块对象字典偏移位置
    memset(&ecOffOd[0], 0, ecatDevs.size()*sizeof(ec_off_od) );

    pAliasSetRegRequest.resize( ecatDevs.size() );
    resetRequestFlag.resize( ecatDevs.size() );
    ampModeDisp.resize( ecatDevs.size() );
    pHomeOffsetSdoRequest.resize( ecatDevs.size() );
    zeroValue.resize(ecatDevs.size());
    oldZeroValue.resize(ecatDevs.size());
    nodeToRobotIdAndAxit.resize( ecatDevs.size() );

    sdo_request_603f.resize( ecatDevs.size() );
    sdo_request_alias.resize( ecatDevs.size() );
    slave_sc.resize( ecatDevs.size() );

    isOp.resize( ecatDevs.size() );
    for(int i=0;i<isOp.size();i++){
        isOp[i]=false;
    }

    ///API 相关
    servoStateData.resize(ecatDevs.size());
    ioDeviceData.resize(ioDevAlias.size());
    qDebug()<< ioDevAlias;
    int tAlias=0;
    RobotInfo tRob;
    foreach(tRob,robots)
    {
        QVector<ServoState*>  pData;
        pData.resize( tRob.motorAlias.size() );

        for(int i=0; i<tRob.motorAlias.size(); i++ )
        {
            tAlias= tRob.motorAlias.at(i);
            int nd=aliasMapNode.value( tAlias );
            pData[i]= &(servoStateData[nd]);

            nodeToRobotIdAndAxit[nd].axis=nd;
            nodeToRobotIdAndAxit[nd].robotId=tRob.robotId;
        }
        //       qDebug()<<tRob.robotId << ioDevAlias.size() << tRob.motorAlias.size()<< servoStateData.size();
        servoStateMap.insert(  tRob.robotId, pData  );
        robotServoOnFlag.insert( tRob.robotId, false  );
    }

}

//检查从站配置错误（配置后检测）
bool CoeEcatDevice:: checkEcatConfigErr()
{
    for(int i=0;i<robotUseAliasList.size();i++)
    {
        S_EcatDevInfo* robDev=robotDevs.value( robotUseAliasList.at(i) ,NULL);
        if(robDev==NULL)
        {
            qDebug()<<"robDev NULL Err:"<<__LINE__;
            return false;
        }

        if(robDev->devInfo.devType==E_EcatDev_Servo||robDev->devInfo.devType==E_EcatDev_IoModule)
        {
            bool ok=false;
            char cmdBuf[100]={0};
            char returnBuf[100]={0};

            int nd = aliasMapNode.value(robotUseAliasList.at(i));
            sprintf(cmdBuf, "ethercat pdos -p %d | grep entry | sed -n '$='", nd );
            getResultFromSystemCall( cmdBuf, returnBuf, 100);
            int entrycount=QString(returnBuf).toInt(&ok);

            if(entrycount != robDev->objConfig.ec_entries.size())
            {
                qDebug()<<"ethercat slaves config error:"<<nd << robotUseAliasList.at(i)<<entrycount <<robDev->objConfig.ec_entries.size();
                return false;
            }
        }
    }

    return true;
}

void CoeEcatDevice::displayBusDevicesInfo( )
{
#if !D_IF_DEBUG
    return;
#endif
    qDebug()<<    "\n========displayDevicesInfo=========  "  ;
    qDebug()<<    "Ethercat总线上所有设备="<<ecatDevs.size();
    qDebug()<<    "已经使用的设备="<<robotDevs.size();
    qDebug()<<    "机器人数="<<robots.size();
    qDebug()<<    "电机数="<<motorCount;
    qDebug()<<    "对象字典数="<<entriesCount;
    qDebug()<<    "别名 ->设备:"<< aliasMapDeviceType;
    qDebug()<<    "别名 ->站点:"<< aliasMapNode;
    qDebug()<<    "busXml->aliasList="<<busXml->aliasList;
    qDebug()<<    "robotUseAliasList="<<robotUseAliasList ;

    for(int i=0;i< ecatDevs.size();i++)
    {
        switch( ecatDevs.at(i).devInfo.devType )
        {
        case  E_EcatDev_TypeErr:
            qDebug()<<"设备:"<< i <<"不可用*";
            break;
        case  E_EcatDev_NotUsed:
            qDebug()<<"设备:"<< i <<"未使用*";
            break;
        case  E_EcatDev_New:
            qDebug()<<"设备:"<< i <<"新增*";
            break;
        case  E_EcatDev_Used:
            qDebug()<<"设备:"<< i <<"已使用";
            break;
        case  E_EcatDev_Coupler:
            qDebug()<<"设备:"<< i <<"耦合器";
            break;
        case  E_EcatDev_Servo:
            qDebug()<<"设备:"<< i <<"伺服";
            break;
        case  E_EcatDev_IoModule:
            qDebug()<<"设备:"<< i <<"io模块";
            break;
        default:
            break;
        }
        qDebug()<< ecatDevs.at(i).devInfo.devName;
    }
}

void CoeEcatDevice::displayDomainRegConfig(  )
{
#if !D_IF_DEBUG
    return;
#endif

    int domainSize = ecrt_domain_size( domain );

    qDebug()<< "\n========displayDomainRegConfig=========  "  ;
    qDebug()<< "domainSize="<<domainSize;

    for( int i=0;i<builddomainRegs.size()-1 ; i++)
    {
        qDebug( "\nDomain Regs[ %d ]={ ", i);
        if( builddomainRegs.at(i).offset )
        {
            qDebug() << "index : 0x"+QString("%1").setNum(builddomainRegs.at(i).index, 16)
                     << "subindex : 0x"+QString("%1").setNum(builddomainRegs.at(i).subindex, 16)
                     << "offset :  "+QString("%1").setNum( *(builddomainRegs.at(i).offset), 10 );
        }

        if( builddomainRegs.at(i).bit_position )
        {
            qDebug() << "bit_position :  "+QString("%1").setNum( *(builddomainRegs.at(i).bit_position), 10 );
        }
        qDebug() << " }";
    }

}

void CoeEcatDevice::displayEthercatConfig( int nd  )
{
#if !D_IF_DEBUG
    return;
#endif

    if( nd >=ecatDevs.size() )
        return;

#if 0
    int pdoOffset=0;
    int entryOffset=0;

    qDebug()<< "\n================= dev:" << nd ;
    qDebug()<< "Slave Alias="<<nodeMapAlias.value(nd);
    qDebug()<< "Dev devName="<< ecatDevs[nd].devInfo.devName;
    qDebug()<< "Dev Type="<< ecatDevs[nd].devInfo.devType;
    qDebug()<< "Vendor Id="<<"0x"+QString("%1").setNum(ecatDevs[nd].devInfo.vendorId, 16);
    qDebug()<< "Product Code="<<"0x"+QString("%1").setNum(ecatDevs[nd].devInfo.productCode, 16);
    qDebug()<< "Revision Number="<<"0x"+QString("%1").setNum(ecatDevs[nd].devInfo.revisionNumber, 16);

    qDebug()<< "\n";
    qDebug( "ec_entries[ %d ]=",  nd) ;
    for(int j=0;j<ecatDevs[nd].objConfig.ec_entries.size(); j++)
    {
        qDebug()<< "    { "
                << "0x"+QString("%1").setNum(ecatDevs[nd].objConfig.ec_entries.at(j).index, 16)
                << "0x"+QString("%1").setNum(ecatDevs[nd].objConfig.ec_entries.at(j).subindex, 16)
                << ecatDevs[nd].objConfig.ec_entries.at(j).bit_length
                << " }";
    }

    qDebug()<< "\n";
    qDebug( "ec_pdos[ %d ]=" , nd) ;
    for(int j=0;j<ecatDevs[nd].objConfig.ec_pdos.size(); j++)
    {
        for(int k=0;k<ecatDevs[nd].objConfig.ec_entries.size();k++ )
        {
            if( ecatDevs[nd].objConfig.ec_pdos.at(j).entries== &(ecatDevs[nd].objConfig.ec_entries[k]) )
            {
                entryOffset= k;
                // qDebug()<< "entryOffset="<<entryOffset;
                break;
            }
        }
        qDebug()<< "    { "
                << "0x"+QString("%1").setNum(ecatDevs[nd].objConfig.ec_pdos.at(j).index, 16)
                << ecatDevs[nd].objConfig.ec_pdos.at(j).n_entries
                << "ec_entries+"+QString::number(entryOffset)
                << " }";
    }

    QString dir;
    QString wd;
    qDebug()<<"\n";
    qDebug("ec_syncs[ %d ]=", nd) ;


    for(int j=0;j<ecatDevs[nd].objConfig.ec_syncs.size(); j++)
    {
        for(int k=0;k<ecatDevs[nd].objConfig.ec_pdos.size();k++ )
        {
            if( ecatDevs[nd].objConfig.ec_syncs.at(j).pdos== &(ecatDevs[nd].objConfig.ec_pdos[k]) )
            {
                pdoOffset= k;
                break;
            }
        }
        if( ecatDevs[nd].objConfig.ec_syncs.at(j).watchdog_mode==EC_WD_DEFAULT )
            wd= "EC_WD_DEFAULT";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).watchdog_mode==EC_WD_ENABLE )
            wd= "EC_WD_ENABLE";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).watchdog_mode==EC_WD_DISABLE )
            wd= "EC_WD_DISABLE";

        if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_INVALID )
            dir= "EC_DIR_INVALID";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_OUTPUT )
            dir= "EC_DIR_OUTPUT";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_INPUT )
            dir= "EC_DIR_INPUT";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_COUNT )
            dir= "EC_DIR_COUNT";

        qDebug()<< "    { "
                << ecatDevs[nd].objConfig.ec_syncs.at(j).index
                << dir
                << ecatDevs[nd].objConfig.ec_syncs.at(j).n_pdos
                << "ec_pdos+"+QString::number(pdoOffset)
                << wd
                << " }";
    }

#else

    qDebug()<< "\n================= dev:" << nd ;
    qDebug()<< "Slave Alias="<<nodeMapAlias.value(nd);
    qDebug()<< "Dev Type="<< ecatDevs[nd].devInfo.devType;
    qDebug()<< "Vendor Id="<<"0x"+QString("%1").setNum(ecatDevs[nd].devInfo.vendorId, 16);
    qDebug()<< "Product Code="<<"0x"+QString("%1").setNum(ecatDevs[nd].devInfo.productCode, 16);
    qDebug()<< "Revision Number="<<"0x"+QString("%1").setNum(ecatDevs[nd].devInfo.revisionNumber, 16);
    qDebug()<< "\n";
    qDebug( "ec_entries[ %d ]=",  nd) ;

    int tcount=0;
    for(int j=0;j<ecatDevs[nd].objConfig.ec_syncs.size(); j++)
    {
        for(int k=0;k<ecatDevs[nd].objConfig.ec_syncs.at(j).n_pdos;k++)
        {
            for(int n=0;n<ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].n_entries;n++)
            {
                if( (ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].entries+n)!=NULL )
                {
                    qDebug()<<  tcount++<<"    { "
                             << "0x"+QString("%1").setNum(ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].entries[n].index, 16)
                             << "0x"+QString("%1").setNum(ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].entries[n].subindex, 16)
                             << ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].entries[n].bit_length
                             << " }";
                }
            }
        }
    }

    tcount=0;
    int entryOffset=0;
    qDebug()<< "\n";
    qDebug( "ec_pdos[ %d ]=" , nd) ;
    for(int j=0;j<ecatDevs[nd].objConfig.ec_syncs.size(); j++)
    {
        for(int k=0;k<ecatDevs[nd].objConfig.ec_syncs.at(j).n_pdos;k++)
        {
            if( (ecatDevs[nd].objConfig.ec_syncs.at(j).pdos+k) !=NULL)
            {
                for(int n=0;n<ecatDevs[nd].objConfig.ec_entries.size();n++ )
                {
                    if( ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].entries== &(ecatDevs[nd].objConfig.ec_entries[n]) )
                    {
                        entryOffset= n;
                        break;
                    }
                }

                qDebug()<< tcount++<< "    { "
                        << "0x"+QString("%1").setNum(ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].index, 16)
                        << ecatDevs[nd].objConfig.ec_syncs.at(j).pdos[k].n_entries
                        << "ec_entries+"+QString::number(entryOffset)
                        << " }";
            }
        }
    }

    tcount=0;
    int pdoOffset=0;
    QString dir;
    QString wd;
    qDebug()<<"\n";
    qDebug("ec_syncs[ %d ]=", nd) ;
    for(int j=0;j<ecatDevs[nd].objConfig.ec_syncs.size(); j++)
    {
        for(int k=0;k<ecatDevs[nd].objConfig.ec_pdos.size();k++ )
        {
            if( ecatDevs[nd].objConfig.ec_syncs.at(j).pdos== &(ecatDevs[nd].objConfig.ec_pdos[k]) )
            {
                pdoOffset= k;
                break;
            }
        }
        if( ecatDevs[nd].objConfig.ec_syncs.at(j).watchdog_mode==EC_WD_DEFAULT )
            wd= "EC_WD_DEFAULT";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).watchdog_mode==EC_WD_ENABLE )
            wd= "EC_WD_ENABLE";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).watchdog_mode==EC_WD_DISABLE )
            wd= "EC_WD_DISABLE";

        if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_INVALID )
            dir= "EC_DIR_INVALID";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_OUTPUT )
            dir= "EC_DIR_OUTPUT";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_INPUT )
            dir= "EC_DIR_INPUT";
        else if( ecatDevs[nd].objConfig.ec_syncs.at(j).dir==EC_DIR_COUNT )
            dir= "EC_DIR_COUNT";

        if(ecatDevs[nd].objConfig.ec_syncs.at(j).pdos!=NULL && ecatDevs[nd].objConfig.ec_syncs.at(j).n_pdos)
            // if(ecatDevs[nd].objConfig.ec_syncs.at(j).pdos!=NULL )
        {
            qDebug()<< tcount++<<"    { "
                    << ecatDevs[nd].objConfig.ec_syncs.at(j).index
                    << dir
                    << ecatDevs[nd].objConfig.ec_syncs.at(j).n_pdos
                    << "ec_pdos+"+QString::number(pdoOffset)
                    << wd
                    << " }";
        }else{
            qDebug()<< tcount++<<"    { "
                    << ecatDevs[nd].objConfig.ec_syncs.at(j).index
                    << dir
                    << ecatDevs[nd].objConfig.ec_syncs.at(j).n_pdos
                    << "NULL"
                    << wd
                    << " }";
        }
    }
#endif
    qDebug()<< "\n===== display config end =======\n "   ;
}

bool CoeEcatDevice::detectionEcatBusDevs( )
{
    ///2017-1-17-xyf, 测试发现: 设备启动过程，出现只有一个设备，且是可用状态. 所以修改为更加可靠的判定
    //读取总线配置文件
    if(busXml==NULL){
        busXml=new EcatBusXml(D_ECATBUS_CONFIG_FILE);
    }

    if(!busXml->readBusConfig())
        return false;

    ioDevAlias.clear();
    robotUseAliasList.clear();
    int devcount=busXml->aliasList.size();
    for(int i=0;i<devcount;i++)
    {
        int tAlias=busXml->aliasList.at(i);
        int type= busXml->aliasMapDeviceType.value( tAlias, E_EcatDev_TypeErr );
        if( type == E_EcatDev_Coupler||  type ==E_EcatDev_IoModule)
        {
            ioDevAlias.append( tAlias );
        }
        robotUseAliasList.append( tAlias );
    }

    //检测总线设备
    bool ok=false;
    char cmdBuf[100]={0};
    char returnBuf[100]={0};
    int timeCount = 0;
    int breakCount=100;//30S
    int tmpDevcount = 0;
    do{
        ok=false;

        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(returnBuf, 0, 100*sizeof(char) );
        sprintf( cmdBuf, "ethercat slaves | sed -n '$='" );
        getResultFromSystemCall( cmdBuf, returnBuf, 100 );
        QString retStr(returnBuf);

        if(retStr.indexOf("Failed to open")>0) {
            qDebug()<<"ethercat 驱动未启动!"<< retStr;
            addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 35, -1  );
            return false;
        }else if( retStr.isEmpty() ){
            if( timeCount>breakCount ){//9s
                qDebug()<<"39 ethercat 设备没有响应, 请检查网卡或网络状态！";
                addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 39, -1  );
            }
            //return false;
        }else{
            bool toIntOK=false;
            tmpDevcount = retStr.toInt(&toIntOK);
            if( toIntOK )
            {
                if( tmpDevcount < devcount )
                {//设备数目不对
                    if( timeCount>breakCount )
                    {
                        //检测超时
                        qDebug()<<"ethercat 检测: 设备数错误!  ";
                        addMsg( ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 37, -1,  tmpDevcount  );
                        return false;
                    }
                }
                else
                {
#if D_IF_DEBUG
                    qDebug()<<"ethercat 总线检测"<<tmpDevcount<<devcount;
#endif
                    if( tmpDevcount>0 )
                    {
                        ok=true;
                        break;
                    }else{
                        qDebug()<<"ethercat 检测: 设备数错误!  ";
                        addMsg( ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 37, -1 , tmpDevcount );
                        return false;
                    }
                }
            } else{
                qDebug()<<"ethercat 设备检测出现未知错误2！"<< devcount << toIntOK <<retStr ;
                addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 38, -1  );
                return false;
            }
        }

        timeCount++;
        sleep_ns_real_time(300000000);//300ms
    }while( !ok );

    nodeMapAlias.clear();
    ecatDevs.clear();
    ecatDevs.resize( tmpDevcount );
    aliasMapNode.clear();

    timeCount=0;
    for(int i=0; i<ecatDevs.size(); i++)
    {
        bool isDevReady=false;
        do{
            memset(cmdBuf, 0, 100*sizeof(char) );
            memset(returnBuf, 0, 100*sizeof(char) );
            sprintf( cmdBuf, "ethercat slaves | sed -n %dp", i+1 );
            getResultFromSystemCall( cmdBuf, returnBuf, 100 );

            QString retStr(returnBuf);
            if( retStr.indexOf("PREOP")>0 || retStr.indexOf("SAFEOP")>0 )
            {
                isDevReady=true;
            }
            else
            {
                //  rt_printf("从站 %d 还未准备就绪！\n", i );
                isDevReady=false;
                sleep_ns_real_time( 500000000 );//500ms

                timeCount++;
                if( timeCount>10 )
                {
                    qDebug()<< "ethercat 设备启动失败, 请尝试重启！"<< i <<retStr;
                    addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 36, -1, i  );
                    return false;
                }
            }
        }while( !isDevReady );

        ecatDevs[i].devInfo.node = i;

        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(returnBuf, 0, 100*sizeof(char) );
        sprintf(cmdBuf, "ethercat slave -p %d -v | grep Alias" , i );
        getResultFromSystemCall( cmdBuf, returnBuf, 100);

        QString Alias(returnBuf);
        ecatDevs[i].devInfo.alias=Alias.remove("Alias:").toInt();
        if( aliasMapNode.contains(ecatDevs[i].devInfo.alias)  )
        {
            addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 33, ecatDevs[i].devInfo.alias, i );
        }

        aliasMapNode.insertMulti(ecatDevs[i].devInfo.alias,i);
        nodeMapAlias.insert(i,ecatDevs[i].devInfo.alias);

        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(returnBuf, 0, 100*sizeof(char) );
        sprintf(cmdBuf, "ethercat slave -p %d -v | grep Device" , i );
        getResultFromSystemCall( cmdBuf, returnBuf, 100);
        QString Device(returnBuf);
        ecatDevs[i].devInfo.devName=Device.remove("Device name:").remove("\n");

        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(returnBuf, 0, 100*sizeof(char) );
        sprintf(cmdBuf, "ethercat slave -p %d -v | grep Vendor" , i );
        getResultFromSystemCall( cmdBuf, returnBuf, 100);
        QString Vendor(returnBuf);
        ecatDevs[i].devInfo.vendorId=Vendor.remove("Vendor Id:").remove(" ").toUInt(&ok,16);

        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(returnBuf, 0, 100*sizeof(char) );
        sprintf(cmdBuf, "ethercat slave -p %d -v | grep Product" , i );
        getResultFromSystemCall( cmdBuf, returnBuf, 100);
        QString Product(returnBuf);
        ecatDevs[i].devInfo.productCode=Product.remove("Product code:").remove(" ").toUInt(&ok,16);

        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(returnBuf, 0, 100*sizeof(char) );
        sprintf(cmdBuf, "ethercat slave -p %d -v | grep Revision" , i );
        getResultFromSystemCall( cmdBuf, returnBuf, 100);
        QString Revision(returnBuf);
        ecatDevs[i].devInfo.revisionNumber=Revision.remove("Revision number:").remove(" ").toUInt(&ok,16);

        ecatDevs[i].devInfo.devType=E_EcatDev_New;
        ecatDevs[i].devInfo.mappingObjectFile="";
    }

    return true;
}

//template <class Key, class T>
static void stdMapToQmap( const  std::map<int, RobotInfo> &stdMap ,    QMap<int, RobotInfo> &qMap )
{
    /*  stdMap.at(i)
     for(int i=0; i<stdMap.size(); i++ )
            {
         stdMap.find()
     }
    foreach(stdMap ,stdMap )
            {
           cout<<pair.first<<":"<<pair.second<<endl;
       });

    std::map<int, int> map;
    std::for_each(stdMap.begin(), stdMap.end(), [](const pair<char*,int> &pair)
    {
          cout<<pair.first<<":"<<pair.second<<endl;
      });

    std::map<T,T>::iterator it;
      for(it=m.begin();it!=m.end();++it)
          cout<<"key: "<<it->first <<" value: "<<it->second<<endl;
  */

    if(stdMap.size()>0)
    {
        qMap.clear();
        for( std::map< int, RobotInfo >::const_iterator iter  = stdMap.begin(); iter != stdMap.end(); iter++)
        {
            qMap.insert( iter->first, iter->second );
            // 打印遍历结果
        }
    }

}

//读取机器人信息(个数和使用的 ethercat 配置文件)
bool CoeEcatDevice::readRobotsConfig()
{

#if D_TEST_NOT_READ_ROBOT_FILE
    //    if( robotXml==NULL  )
    {
        //        qDebug()<<"Object Error: *robotXml ==NULL";
        //        return false;
        //    }
        //    std::map<int, RobotInfo> std_robots; //所有机器人信息 <机器人ID，信息>
        //    std_robots=robotXml->readRobotsConfig(  );//   bool ok

        //    bool ok=std_robots.size()>0?true:false;
        //    if(ok)
        //    {
        //        stdMapToQmap(std_robots, robots );
        //    }

        bool ok=true;
        RobotInfo robInfo;
        robInfo.robotId = 1;
        robInfo.doCount = 16;
        robInfo.diCount = 16;
        robInfo.robotMotorCount = 4;
        robInfo.extMotorCount = 0;

        robInfo.robotFolder = "robotData1";
        robInfo.robotSetting = "robotSetting.xml";
        robInfo.ioMapXML = "iomap.xml";

        //    //SX
        //    robInfo.motorAlias.append( 91 );
        //    robInfo.motorAlias.append( 92 );
        //    robInfo.motorAlias.append( 93 );
        //    robInfo.motorAlias.append( 94 );

        //LS
        robInfo.motorAlias.append( 81 );
        robInfo.motorAlias.append( 82 );
        robInfo.motorAlias.append( 83 );
        robInfo.motorAlias.append( 84 );

        eCat->robots.insert(robInfo.robotId,  robInfo);

#else
    bool ok=false;
    if( robots.size()<1  )
    {
        rt_printf( "robots size error: 0 " );
        ok= false;
    }else{
        ok= true;
    }
#endif

    robotIdKeys=robots.keys();
    if(ok)
    {
        int tAlias=0;
        RobotInfo tRob;
        foreach(tRob,robots)
        {
            foreach(tAlias,tRob.motorAlias)
            {
                if( !robotUseAliasList.contains(tAlias) )
                {
                    robotUseAliasList.append(tAlias);
                }
            }
        }
    }

    return ok;
}




/**
 * @brief CoeEcatDevice::resetDeviceType  设备检测标识
 * //要考虑 如果设备不存在，却有有配置文件或robot用到的情况

设备检测标识过程:
            |============检测到的总线所有设备===============|
            |=============================================|新增
            |===============================|未使用
|----------- ===============|已使用
|====Err====|---------------|

 */
void CoeEcatDevice::resetDeviceType()
{
    int tAlias=0;

    aliasMapDeviceType.clear();
    for(int i=0;i< ecatDevs.size();i++)
    {
        tAlias=ecatDevs.at(i).devInfo.alias;
        aliasMapDeviceType.insert( tAlias, E_EcatDev_New );
    }

    foreach(tAlias,busXml->aliasList)
    {
        if( aliasMapDeviceType.contains( tAlias ))
        {
            aliasMapDeviceType.insert( tAlias, E_EcatDev_NotUsed );
        }else{
            qDebug( "Message: Error: 无效的总线设备, 别名:%d ！" , tAlias );
            addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 17, -1, tAlias );
            aliasMapDeviceType.insert( tAlias, E_EcatDev_TypeErr );//设备无效
        }
    }

    foreach(tAlias, robotUseAliasList)
    {
        if( aliasMapDeviceType.contains( tAlias )
                && aliasMapDeviceType.value(tAlias)!=E_EcatDev_TypeErr)
        {
            aliasMapDeviceType.insert( tAlias, E_EcatDev_Used );
        }else{
            qDebug( "Message: Error: 无效的机器人设备, 别名:%d ！" , tAlias );
            addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 18, -1, tAlias );
            aliasMapDeviceType.insert( tAlias, E_EcatDev_TypeErr );//设备无效
        }
    }

    robotDevs.clear();
    for(int i=0; i< ecatDevs.size(); i++)
    {
        tAlias = ecatDevs.at(i).devInfo.alias ;
        if( aliasMapDeviceType.value( tAlias )==E_EcatDev_Used )
        {
            robotDevs.insertMulti( tAlias, &ecatDevs[i] );
        }
    }
}


bool CoeEcatDevice::readEcatBusDevsConfig( )
{

    resetDeviceType();

    for(int i=0;i<busXml->aliasList.size();i++)
    {
        int tAlias=busXml->aliasList.at(i);
        S_EcatDevInfo* robDev=robotDevs.value(tAlias ,NULL);
        if(robDev==NULL )
        {
            continue;
        }
        if( aliasMapDeviceType.value( tAlias, E_EcatDev_TypeErr ) != E_EcatDev_TypeErr )
        {
            robDev->devInfo.devType =  busXml->aliasMapDeviceType.value(tAlias, E_EcatDev_TypeErr)  ;
            if(  robDev->devInfo.devType == E_EcatDev_TypeErr  )
            {
                qDebug()<<"总线配置文件所指定的设备没有找到 ！ " << tAlias;
                addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 6, -1, tAlias );
                return false;
            }
            aliasMapDeviceType.insert( tAlias,  robDev->devInfo.devType );
            int nd = aliasMapNode.value( tAlias );

            QString xmlfile = busXml->aliasMapFile.value(tAlias);
            robDev->devInfo.mappingObjectFile=xmlfile;

#if !D_TEST_NOT_READ_XML_FILE
            if(  E_EcatDev_Servo==robDev->devInfo.devType    )
            {

                //
                int vendor = ecatDevs[nd].devInfo.vendorId;
                switch( vendor )
                {
                case D_VENDOR_ID_TCR:
                {
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(tcr_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(tcr_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(tcr_syncs)/sizeof(ec_sync_info_t)  );

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], tcr_entries, sizeof(tcr_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], tcr_pdos, sizeof(tcr_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], tcr_syncs, sizeof(tcr_syncs) );
                    //    ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                case D_VENDOR_ID_Delta:
                {
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(delta_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(delta_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(delta_syncs)/sizeof(ec_sync_info_t)  );

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], delta_entries, sizeof(delta_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], delta_pdos, sizeof(delta_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], delta_syncs, sizeof(delta_syncs) );
                    //    ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                case D_VENDOR_ID_Sanyo:
                {
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(sanyo_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(sanyo_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(sanyo_syncs)/sizeof(ec_sync_info_t)  );
                    //   ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], sanyo_entries, sizeof(sanyo_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], sanyo_pdos, sizeof(sanyo_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], sanyo_syncs, sizeof(sanyo_syncs) );

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                case D_VENDOR_ID_LS:
                {
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(ls_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(ls_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(ls_syncs)/sizeof(ec_sync_info_t)  );
                    //   ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], ls_entries, sizeof(ls_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], ls_pdos, sizeof(ls_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], ls_syncs, sizeof(ls_syncs) );

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs.resize(4);
                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                case D_VENDOR_ID_Panasonic:
                {
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(sx_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(sx_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(sx_syncs)/sizeof(ec_sync_info_t)  );
                    // ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], sx_entries, sizeof(sx_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], sx_pdos, sizeof(sx_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], sx_syncs, sizeof(sx_syncs) );

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs.resize(4);
                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                case D_VENDOR_ID_Invt:
                {
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(invt_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(invt_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(invt_syncs)/sizeof(ec_sync_info_t)  );
                    //    ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], invt_entries, sizeof(invt_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], invt_pdos, sizeof(invt_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], invt_syncs, sizeof(invt_syncs) );

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                default:
                {
                    qDebug()<<"\n警告:使用默认的伺服配置参数: 站号="<<nd;
                    ecatDevs[nd].objConfig.ec_entries.resize( sizeof(motor_entries)/sizeof(ec_pdo_entry_info_t) );
                    ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(motor_pdos)/sizeof(ec_pdo_info_t)  );
                    ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(motor_syncs)/sizeof(ec_sync_info_t)  );
                    //    ecatDevs[nd].objConfig.ec_syncs.resize(4);

                    memcpy(&ecatDevs[nd].objConfig.ec_entries[0], motor_entries, sizeof(motor_entries) );
                    memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], motor_pdos, sizeof(motor_pdos) );
                    memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], motor_syncs, sizeof(motor_syncs) );

                    ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                    ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                    ecatDevs[nd].objConfig.ec_syncs[0].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[1].pdos=NULL;
                    ecatDevs[nd].objConfig.ec_syncs[2].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                    ecatDevs[nd].objConfig.ec_syncs[3].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
                }
                    break;
                }

                continue;
            }else  if(  E_EcatDev_Coupler==robDev->devInfo.devType  )
            {
                continue;
            }else  if(  E_EcatDev_IoModule==robDev->devInfo.devType )
            {

                //　这个是读取厂商配置文件用到的
                bool ok= readEcatDevSettingsXml(xmlfile , nd) ;
                if( ok==false  )
                {
                    return false;
                }
            }
#else
#if 0
            if( robDev->devInfo.devType==E_EcatDev_Coupler )
            {
                continue;
            }else if( robDev->devInfo.devType==E_EcatDev_IoModule  )
            {
                getEcatDeviceConfig( nd );
            }else if( robDev->devInfo.devType==E_EcatDev_Servo  )
            {
                ecatDevs[nd].objConfig.ec_entries.resize( sizeof(motor_entries)/sizeof(ec_pdo_entry_info_t) );
                ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(motor_pdos)/sizeof(ec_pdo_info_t)  );
                ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(motor_syncs)/sizeof(ec_sync_info_t)  );

                memcpy(&ecatDevs[nd].objConfig.ec_entries[0], motor_entries, sizeof(motor_entries) );
                memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], motor_pdos, sizeof(motor_pdos) );
                memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], motor_syncs, sizeof(motor_syncs) );

                ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                ecatDevs[nd].objConfig.ec_syncs[0].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                ecatDevs[nd].objConfig.ec_syncs[1].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];
            }
#else
            /*
            E_EcatDev_TypeErr=-1,//错误,无效，无法识别,不可用状态
            E_EcatDev_New,//新增,未分类
            E_EcatDev_NotUsed,//
            E_EcatDev_Used,//

            E_EcatDev_Coupler,//耦合器
            E_EcatDev_Servo,//
            E_EcatDev_IoModule,//
*/
            int nd = aliasMapNode.value(tAlias);
            int devtype = 0;

            if(nd<4)
            {
                devtype = E_EcatDev_Servo;

                ecatDevs[nd].objConfig.ec_entries.resize( sizeof(motor_entries)/sizeof(ec_pdo_entry_info_t) );
                ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(motor_pdos)/sizeof(ec_pdo_info_t)  );
                ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(motor_syncs)/sizeof(ec_sync_info_t)  );

                memcpy(&ecatDevs[nd].objConfig.ec_entries[0], motor_entries, sizeof(motor_entries) );
                memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], motor_pdos, sizeof(motor_pdos) );
                memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], motor_syncs, sizeof(motor_syncs) );

                ecatDevs[nd].objConfig.ec_pdos[0].entries=&ecatDevs[nd].objConfig.ec_entries[0];
                ecatDevs[nd].objConfig.ec_pdos[1].entries=&ecatDevs[nd].objConfig.ec_entries[ecatDevs[nd].objConfig.ec_pdos[0].n_entries];

                ecatDevs[nd].objConfig.ec_syncs[0].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                ecatDevs[nd].objConfig.ec_syncs[1].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];

            }else if(nd==4 )
            {
                devtype = E_EcatDev_Coupler;
            }else if(nd==5 )
            {
                devtype = E_EcatDev_IoModule;

                ecatDevs[nd].objConfig.ec_entries.resize( sizeof(slave_t5_pdo_entries)/sizeof(ec_pdo_entry_info_t) );
                ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(slave_t5_pdos)/sizeof(ec_pdo_info_t)  );
                ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(slave_t5_syncs)/sizeof(ec_sync_info_t)  );

                memcpy(&ecatDevs[nd].objConfig.ec_entries[0], slave_t5_pdo_entries, sizeof(slave_t5_pdo_entries) );
                memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], slave_t5_pdos, sizeof(slave_t5_pdos) );
                memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], slave_t5_syncs, sizeof(slave_t5_syncs) );

                for(int k=0;k<16;k++ )
                {
                    ecatDevs[nd].objConfig.ec_pdos[ k].entries=&ecatDevs[nd].objConfig.ec_entries[ k];
                }

                ecatDevs[nd].objConfig.ec_syncs[0].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
                ecatDevs[nd].objConfig.ec_syncs[1].pdos=&ecatDevs[nd].objConfig.ec_pdos[1];

            }else if(nd==6 )
            {

                devtype = E_EcatDev_IoModule;
                ecatDevs[nd].objConfig.ec_entries.resize( sizeof(slave_t6_pdo_entries)/sizeof(ec_pdo_entry_info_t) );
                ecatDevs[nd].objConfig.ec_pdos.resize( sizeof(slave_t6_pdos)/sizeof(ec_pdo_info_t)  );
                ecatDevs[nd].objConfig.ec_syncs.resize( sizeof(slave_t6_syncs)/sizeof(ec_sync_info_t)  );

                memcpy(&ecatDevs[nd].objConfig.ec_entries[0], slave_t6_pdo_entries, sizeof(slave_t6_pdo_entries) );
                memcpy(&ecatDevs[nd].objConfig.ec_pdos[0], slave_t6_pdos, sizeof(slave_t6_pdos) );
                memcpy(&ecatDevs[nd].objConfig.ec_syncs[0], slave_t6_syncs, sizeof(slave_t6_syncs) );

                for(int k=0;k<16;k++ )
                {
                    ecatDevs[nd].objConfig.ec_pdos[ k].entries=&ecatDevs[nd].objConfig.ec_entries[ k];
                }
                ecatDevs[nd].objConfig.ec_syncs[0].pdos=&ecatDevs[nd].objConfig.ec_pdos[0];
            }

            robDev->devInfo.devType=devtype;//(E_EcatDevType)
            aliasMapDeviceType.insert( tAlias, robDev->devInfo.devType );
#endif
#endif
        }
    }
    return true;
}



//三个结构体， 用于记录pdo，sync，entry的关系，重新设置指针指向正确数据
struct SmyPdo{
    int pdo_index;
    int entry_start;
    int entry_count;
};
struct SmySync{
    int sync_index;
    int pdo_start;
    int pdo_count;
};
struct SmyEthercatConfig{
    QVector<SmyPdo> pdo;
    QVector<SmySync> sync;
};

//从厂商配置文件读取配置信息 (IO module),
bool CoeEcatDevice::readEcatDevSettingsXml( QString xmlfile,  int nd )
{

    bool ok=false;
    ok=openXml(xmlfile,QFile::ReadWrite | QFile::Text);
    if( !ok  )
    {
        qDebug()<<"从厂商配置文件读取配置信息失败，无法打开文件: node= "<<nd <<xmlfile;
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 7, -1, nd );
        return false;
    }

    unsigned int tmpInt=0;
    QStringList strList;
#ifndef SIMULATION_MODE
    strList<<"Vendor"<< "Id";
    ok=readXml(  findNode( strList, ok ) , tmpInt );
    if( tmpInt!=ecatDevs[nd].devInfo.vendorId  )
    {
        qDebug()<<"从厂商配置文件读取配置信息失败，厂商ID错误: : node= "<<nd  <<ok <<tmpInt << ecatDevs[nd].devInfo.vendorId <<xmlfile;
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 8, -1, nd );
        return false;
    }
#endif
    //   int devtype;
    strList.clear();
    strList<<"Descriptions"<<"Devices";
    QDomNode node= findNode(strList,ok);
    if(!ok )
    {
        qDebug( "read dev setting faile, %d!", nd );
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 9, -1, nd );

        closeXml();
        return false;
    }

    bool isFound=false;
    QString tmpStr;
    QDomNode devnode;
    QDomNodeList nodeList=node.childNodes();
    for(int i=0;i<nodeList.size();i++)
    {
        //        usleep(100);
        //        QCoreApplication::processEvents();

        devnode = nodeList.at(i);
        //  qDebug()<< "find : nodeList.at(i) :"<<devnode.nodeName() <<i <<node.nodeName();

        QString strProductCode= findSubNode( devnode, "Type", ok ).toElement().attribute("ProductCode");
        QString strRevisionNo= findSubNode( devnode, "Type", ok ).toElement().attribute("RevisionNo");

        unsigned  int produtInt=0;
        unsigned int revisionInt=0;

        if( strProductCode.indexOf("0x")>=0 ||  strProductCode.indexOf("#x")>=0 )
            produtInt = strProductCode.remove("#x").toUInt(&ok,16);
        else
            produtInt = strProductCode.toUInt(&ok);

        if( strRevisionNo.indexOf("0x")>=0 ||  strRevisionNo.indexOf("#x")>=0 )
            revisionInt = strRevisionNo.remove("#x").toUInt(&ok,16);
        else
            revisionInt = strRevisionNo.toUInt(&ok);

#ifndef SIMULATION_MODE
        if( ecatDevs[nd].devInfo.productCode!= produtInt
                || ecatDevs[nd].devInfo.revisionNumber  != revisionInt )
        {
            //  qDebug()<<"productCode error: "<< i<< nd<<ecatDevs[nd].devInfo.productCode<< produtInt <<strProductCode ;
            //  qDebug()<<"revisionNumber error: "<<i<<nd<< ecatDevs[nd].devInfo.revisionNumber<< revisionInt << strRevisionNo;
            continue;
        } else {
            isFound = true;
        }
#else
         isFound = true;
#endif

#if 0
        ok = readXml( findSubNode( devnode, "Type", ok ), tmpStr );
        // check dev name ,有些设备可能名称对不上
        if( ecatDevs[nd].devInfo.devName.indexOf(tmpStr)<0 )
            continue;
#endif
        if( ecatDevs[nd].devInfo.devType==E_EcatDev_IoModule )
        {
            //  qDebug()<<"io device...";
            int smCount = 0;
            int pdoCount = 0;
            int entryCont = 0;
            QDomNode smNode=findSubNode( devnode, "Sm", ok );
            SmyEthercatConfig myEcatConfig;

            // 预先初始化大小，避免内存多次拷贝
            ecatDevs[nd].objConfig.ec_pdos.resize( D_IO_INDEX_MAX );
            ecatDevs[nd].objConfig.ec_entries.resize( D_IO_INDEX_MAX );
            if(ok )
            {
#if 0
                QDomNode fNode=smNode;
                while(!smNode.isNull() && smNode.nodeName().compare("Sm")==0  )
                {
                    smCount++;
                }
                ecatDevs[nd].objConfig.ec_syncs.resize(smCount);
                smCount=0;
                smNode=fNode;
#endif

                while( !smNode.isNull() && smNode.nodeName().compare("Sm")==0 )
                {
                    ecatDevs[nd].objConfig.ec_syncs.resize(smCount+1);
                    readXml(smNode,tmpStr);
                    ecatDevs[nd].objConfig.ec_syncs[smCount].n_pdos=0;
                    ecatDevs[nd].objConfig.ec_syncs[smCount].index=smCount;
                    ecatDevs[nd].objConfig.ec_syncs[smCount].dir=EC_DIR_INVALID;
                    ecatDevs[nd].objConfig.ec_syncs[smCount].watchdog_mode=EC_WD_DISABLE;
                    ecatDevs[nd].objConfig.ec_syncs[smCount].pdos=NULL;
                    smNode=smNode.nextSibling();
                    smCount++;
                }

              // qDebug()<<"smCount="<<smCount;

                QDomNode pdoNode=findSubNode( devnode, "RxPdo", ok );
                if(ok)
                {
                    while( !pdoNode.isNull() && pdoNode.nodeName().compare("RxPdo")==0 )
                    {
                        int sm = pdoNode.toElement().attribute("Sm").toInt(&ok);
                        if(!ok )
                        {
                            pdoNode=pdoNode.nextSibling();
                            continue;
                        }
                        if( sm< ecatDevs[nd].objConfig.ec_syncs.size() )
                        {
                            ecatDevs[nd].objConfig.ec_syncs[sm].dir=EC_DIR_OUTPUT;
                            /// 2016.11.10.xyf,经测试 //必须使用EC_WD_ENABLE才能写 DO
                            if( ecatDevs[nd].devInfo.devType==E_EcatDev_IoModule )
                                ecatDevs[nd].objConfig.ec_syncs[sm].watchdog_mode=EC_WD_ENABLE;
                        }

                        //  ecatDevs[nd].objConfig.ec_pdos.resize( 16 );
                        // ecatDevs[nd].objConfig.ec_pdos.resize(pdoCount+1);
                        ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries=0;

                        readXml( findSubNode( pdoNode,"Index", ok )
                                 ,(int&)ecatDevs[nd].objConfig.ec_pdos[pdoCount].index );

                        QDomNode entryNode=findSubNode( pdoNode, "Entry", ok );
                        if(!ok )
                        {
                            qDebug( "read dev setting file faile, %d!", nd );
                            closeXml();
                            return false;
                        }

                        while( !entryNode.isNull() && entryNode.nodeName().compare("Entry")==0 )
                        {
                            //  ecatDevs[nd].objConfig.ec_entries.resize( 16 );
                            // ecatDevs[nd].objConfig.ec_entries.resize(entryCont+1);

                            readXml( findSubNode(entryNode,"Index", ok )
                                     ,(int&)ecatDevs[nd].objConfig.ec_entries[entryCont].index);
                            readXml( findSubNode(entryNode,"SubIndex", ok )
                                     ,ecatDevs[nd].objConfig.ec_entries[entryCont].subindex);
                            readXml( findSubNode(entryNode,"BitLen", ok )
                                     ,ecatDevs[nd].objConfig.ec_entries[entryCont].bit_length);

                            readXml( findSubNode(entryNode,"Name", ok ), tmpStr);
                            if( tmpStr.indexOf("analog")>=0 )
                            {
                                IoModuleAiAoInfo aiaoInfo;
                                aiaoInfo.node = nd;
                                aiaoInfo.index = ecatDevs[nd].objConfig.ec_entries[entryCont].index;
                                aiaoInfo.subindex = ecatDevs[nd].objConfig.ec_entries[entryCont].subindex;
                                aiaoInfo.dir = EC_DIR_INPUT;
                                AiAoObjectInfo.append( aiaoInfo );
                            } else  if( tmpStr.indexOf("dig")>=0 )
                            {
                                ;//
                            }


                            ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries++;
                            entryCont++;
                            entryNode=entryNode.nextSibling();
                        }

                        ecatDevs[nd].objConfig.ec_pdos[pdoCount].entries
                                = &(ecatDevs[nd].objConfig.ec_entries[entryCont-ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries]);

                        ecatDevs[nd].objConfig.ec_syncs[sm].pdos
                                = &(ecatDevs[nd].objConfig.ec_pdos[pdoCount-ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos]);

                        ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos++;

                        myEcatConfig.pdo.resize(pdoCount+1);
                        myEcatConfig.pdo[pdoCount].pdo_index=pdoCount;
                        myEcatConfig.pdo[pdoCount].entry_start=entryCont-ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries;
                        myEcatConfig.pdo[pdoCount].entry_count=ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries;

                        myEcatConfig.sync.resize(sm+1);
                        myEcatConfig.sync[sm].sync_index=sm;
                        myEcatConfig.sync[sm].pdo_start=1+pdoCount-ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos;
                        myEcatConfig.sync[sm].pdo_count=ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos;

//                        qDebug()<< "sm:"<<sm
//                                << "RX pdos:"<<pdoCount
//                                << "entry:" <<myEcatConfig.pdo[pdoCount].entry_count ;

                        pdoCount++;
                        pdoNode=pdoNode.nextSibling();
                    }

                }else{
                    qDebug()<<"no node named <RxPdo> !" <<nd;// <<ecatDevs[nd].devInfo.devName;
                }

                pdoNode=findSubNode( devnode, "TxPdo", ok );
                if(ok )
                {
                    while( !pdoNode.isNull() && pdoNode.nodeName().compare("TxPdo")==0 )
                    {
                        int sm = pdoNode.toElement().attribute("Sm").toInt(&ok);
                        if(!ok )
                        {
                            pdoNode=pdoNode.nextSibling();
                            continue;
                        }
                        if( sm< ecatDevs[nd].objConfig.ec_syncs.size() ){
                            ecatDevs[nd].objConfig.ec_syncs[sm].dir=EC_DIR_INPUT;
                            ecatDevs[nd].objConfig.ec_syncs[sm].watchdog_mode=EC_WD_DISABLE;
                        }

                        //   ecatDevs[nd].objConfig.ec_pdos.resize( 16 );
                        //  ecatDevs[nd].objConfig.ec_pdos.resize(pdoCount+1);
                        ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries=0;

                        readXml( findSubNode( pdoNode,"Index", ok )
                                 ,(int&)ecatDevs[nd].objConfig.ec_pdos[pdoCount].index );

                        QDomNode entryNode=findSubNode( pdoNode, "Entry", ok );
                        if(!ok )
                        {
                            qDebug( "read dev setting file failed, %d!", nd );
                            closeXml();
                            return false;
                        }

                        while( !entryNode.isNull() && entryNode.nodeName().compare("Entry")==0 )
                        {
                            //   ecatDevs[nd].objConfig.ec_entries.resize( 16 );
                            //   ecatDevs[nd].objConfig.ec_entries.resize(entryCont+1);

                            readXml( findSubNode(entryNode,"Index", ok )
                                     ,(int&)ecatDevs[nd].objConfig.ec_entries[entryCont].index);
                            readXml( findSubNode(entryNode,"SubIndex", ok )
                                     ,ecatDevs[nd].objConfig.ec_entries[entryCont].subindex);
                            readXml( findSubNode(entryNode,"BitLen", ok )
                                     ,ecatDevs[nd].objConfig.ec_entries[entryCont].bit_length);
                            readXml( findSubNode(entryNode,"Name", ok ), tmpStr);
                            if( tmpStr.indexOf("analog")>=0 )
                            {
                                IoModuleAiAoInfo aiaoInfo;
                                aiaoInfo.node = nd;
                                aiaoInfo.index = ecatDevs[nd].objConfig.ec_entries[entryCont].index;
                                aiaoInfo.subindex = ecatDevs[nd].objConfig.ec_entries[entryCont].subindex;
                                aiaoInfo.dir = EC_DIR_OUTPUT;
                                AiAoObjectInfo.append( aiaoInfo );
                            }else  if( tmpStr.indexOf("dig")>=0 )
                            {
                                ;//
                            }

                            ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries++;
                            entryCont++;
                            entryNode=entryNode.nextSibling();

                        }
                        ecatDevs[nd].objConfig.ec_pdos[pdoCount].entries
                                = &(ecatDevs[nd].objConfig.ec_entries[entryCont-ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries]);

                        ecatDevs[nd].objConfig.ec_syncs[sm].pdos
                                = &(ecatDevs[nd].objConfig.ec_pdos[pdoCount-ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos]);

                        ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos++;

                        myEcatConfig.pdo.resize(pdoCount+1);
                        myEcatConfig.pdo[pdoCount].pdo_index=pdoCount;
                        myEcatConfig.pdo[pdoCount].entry_start=entryCont-ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries;
                        myEcatConfig.pdo[pdoCount].entry_count=ecatDevs[nd].objConfig.ec_pdos[pdoCount].n_entries;

                        myEcatConfig.sync.resize(sm+1);
                        myEcatConfig.sync[sm].sync_index=sm;
                        myEcatConfig.sync[sm].pdo_start=1+pdoCount-ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos;
                        myEcatConfig.sync[sm].pdo_count=ecatDevs[nd].objConfig.ec_syncs[sm].n_pdos;

//                        qDebug()<< "sm:"<<sm
//                                << "TX pdos:"<<pdoCount
//                                << "entry:" <<myEcatConfig.pdo[pdoCount].entry_count ;

                        pdoCount++;
                        pdoNode=pdoNode.nextSibling();
                    }
                }else{
                    qDebug()<<"no node named <TxPdo> !" <<nd;//ecatDevs[nd].devInfo.devName;
                }

            }else{
                qDebug()<<"no node named <Sm> !" <<nd;//<<ecatDevs[nd].devInfo.devName;
            }

            //重置数据大小
            ecatDevs[nd].objConfig.ec_pdos.resize(pdoCount);
            ecatDevs[nd].objConfig.ec_entries.resize(entryCont);

            //重新设置指针指向正确数据
            for(int j=0; j<myEcatConfig.pdo.size(); j++  )
            {
                //  qDebug()<<myEcatConfig.pdo[j].pdo_index << myEcatConfig.pdo[j].entry_start;
                ecatDevs[nd].objConfig.ec_pdos[myEcatConfig.pdo[j].pdo_index].entries
                        = &ecatDevs[nd].objConfig.ec_entries[myEcatConfig.pdo[j].entry_start];
            }
            for(int i=0; i<myEcatConfig.sync.size(); i++  )
            {
                //   qDebug()<<myEcatConfig.sync[i].sync_index << myEcatConfig.sync[i].pdo_start;
                if( ecatDevs[nd].objConfig.ec_syncs[myEcatConfig.sync[i].sync_index].n_pdos>0){
                    ecatDevs[nd].objConfig.ec_syncs[myEcatConfig.sync[i].sync_index].pdos
                            = &ecatDevs[nd].objConfig.ec_pdos[myEcatConfig.sync[i].pdo_start];
                }else{
                    ecatDevs[nd].objConfig.ec_syncs[myEcatConfig.sync[i].sync_index].pdos=NULL;
                }
            }

        }else if( ecatDevs[nd].devInfo.devType==E_EcatDev_Servo  )
        {
            ;//
        } else{
            qDebug()<<"ethercat bus device name error:  "<<nd<< tmpStr <<ecatDevs[nd].devInfo.devName ;
            closeXml();
            return true;
        }
    }

#if D_IF_DEBUG
    if(isFound){
        qDebug( "read dev setting ok, %d ", nd );
    }
#endif

    closeXml();
    return isFound;

}

//别名检测(判断是否插错网线，识别设备是否正确)
bool CoeEcatDevice::checkAlias( )
{
#if 0
    char aliasBuff[20]={0};
    char cmdBuf[50]={0};
    unsigned int devAlias=0;
    aliasMatchError=0;
    for(int i=0; i<robotUseAliasList.size(); i++)
    {
        sprintf(cmdBuf, "ethercat reg_read -p %d 0x12 2" , i );
        getResultFromSystemCall( cmdBuf, aliasBuff, 20);
        //        QString strAlias2(&aliasBuff[0],20);
        //        qDebug()<< "get :strAlias2="<< strAlias2;

        QString strAlias;
        strAlias.push_back( aliasBuff[7] );
        strAlias.push_back( aliasBuff[8] );
        strAlias.push_back( aliasBuff[2] );
        strAlias.push_back( aliasBuff[3] );

        bool t_ok;
        devAlias = strAlias.toUInt(&t_ok,16);
        if( t_ok && devAlias!=0  )
        {

            if(robotUseAliasList.at(i) != devAlias )
            {
                aliasMatchError |= 1<<i;
                qDebug()<< "found error :aliasMatchError: "<<i <<t_ok<< devAlias
                        <<strAlias<<robotUseAliasList.at(i) <<robotDevs.at(i).devType;
            }
            else
                qDebug()<< "get alias ok:"<<i <<devAlias<< strAlias;
        }else{
            qDebug()<< "get alias err: "<<i <<t_ok<< devAlias<<strAlias<<robotUseAliasList.at(i);
            aliasMatchError |= 1<<i;
        }
    }

    return aliasMatchError==0?true:false;

#else

    /* aliasMatchError=0;
    bool ok=false;
    int pos=0;
    for(int i=0;i<robotUseAliasList.size();i++ )
            {
        unsigned int alias = robotUseAliasList.at(i);

        ok=false;
        for(int j=pos;j<ecatDevs.size();j++ )
            {
            if(ecatDevs[j].devInfo.alias==alias )
            {
                pos=j+1;
                ok=true;
                break;
            }
        }
        if(!ok )
            {
            aliasMatchError |= 0x0001<<i;
        }
    }
    return aliasMatchError==0?true:false;*/


    aliasMatchError=0;

    RobotInfo rob;
    foreach (rob, robots)
    {
        // for(int n=0; n<robots.size(); n++  )  {

        if(rob.motorAlias.size()==0 )
        {
            continue;
        }

        bool ok=false;
        int pos=0;
        for(int i=0; i<rob.motorAlias.size(); i++ )
        {
            unsigned int alias = rob.motorAlias.at(i);

            ok=false;
            for(int j=pos;j<ecatDevs.size();j++)
            {
                if(ecatDevs[j].devInfo.alias==alias)
                {
                    pos=j+1;
                    ok=true;
                    break;
                }
            }
            if(!ok )
            {
                aliasMatchError |= 0x0001<<pos;//pos=slave node
            }
        }

    }

    return aliasMatchError==0?true:false;
#endif

}

bool CoeEcatDevice::initDevice( )
{
    if( master==NULL  )
    {
        rt_printf("error : master==NULL \n" );
        return false;
    }

    qDebug()<<"dst34 " <<robotUseAliasList;
    slaveConfigError=0;
    for( int i = 0; i < robotUseAliasList.size(); i++ )
    {
        //获取从站信息
        ec_slave_config_t *sc = NULL;

        S_EcatDevInfo* eDev=robotDevs.value(robotUseAliasList.at(i));
        if(eDev==NULL)
            return false;

        int node =  aliasMapNode.value(eDev->devInfo.alias);

        qDebug()<< "\n initDevice="<< node   <<i  <<robotUseAliasList.at(i)
                <<eDev->devInfo.alias <<eDev->devInfo.vendorId <<eDev->devInfo.productCode << eDev->devInfo.devType;

        // sc = ecrt_master_slave_config( master, eDev->devInfo.alias , node, eDev->devInfo.vendorId, eDev->devInfo.productCode );
        sc = ecrt_master_slave_config( master, 0 , node, eDev->devInfo.vendorId, eDev->devInfo.productCode );

        if( !sc )
        {
            rt_printf("Slave config fail: %d\n", eDev->devInfo.alias );

            slaveConfigError |= 1<<i;
            continue;
        }else{
          //  sc_ref=sc;
            slave_sc[node]= sc;
        }

        //申请寄存器资源:别名
        regAliasSetRequestRealtime( sc, node );
        //SDO资源申请
        sdo_request_603f[node] = ecrt_slave_config_create_sdo_request(sc, 0x603f, 0x0, 2 );
        if( NULL == sdo_request_603f[node] )
        {
            rt_printf("   alarm code read error: sdo Request fail , -1! \n");
        }
        ecrt_sdo_request_timeout(sdo_request_603f[node], 1000);

        //reg: watch dog
        //        pAliasSetRegRequest[node] = ecrt_slave_config_create_reg_request(sc,2);
        //        if( !pAliasSetRegRequest[node] )
        //        {
        //            rt_printf("pAliasSetRegRequest reg Request fail !");
        //            return false;
        //        }
        //        return true;

        switch( eDev->devInfo.devType )
        {
        case E_EcatDev_Servo:

            sdosConfig( node );//sdo参数
            entriesConfig( node );
            syncsConfig( sc, node );

            break;
        case E_EcatDev_Coupler:
            break;//
        case E_EcatDev_IoModule:

            entriesConfig( node );
            syncsConfig( sc, node );

            break;
        case E_EcatDev_TypeErr:
        default:
            break;
        }
    }

    qDebug()<< "domain regs size:"<<builddomainRegs.size() << offCntKeep;

    int ret=ecrt_domain_reg_pdo_entry_list( domain, builddomainRegs.begin() );   //tcdomainRegs

    if( ret  )
    {
        rt_printf( "PDO entry registration fail: %d!\n",ret);
        slaveConfigError  = 0x01;
        return false;
    }

    if( slaveConfigError ){
        qDebug()<<"slaveConfigError : "<<slaveConfigError   ;
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 19, -1, slaveConfigError );
    }

    return slaveConfigError?false:true;
}


bool CoeEcatDevice::activeMaster( )
{

    int ret = ecrt_master_activate(master);
    if(ret<0 )
    {
        rt_printf( "Active master error: fail to get activate master!\n");
        return false;
    }
    if (!(domain_p = ecrt_domain_data(domain)) )
    {
        rt_printf( "Active master error: fail to get domain data pointer!\n");
        return false;
    }

    clock_gettime(CLOCK_MONOTONIC , &wakeupTime);//
    return true;
}


void CoeEcatDevice::sdosConfig( int nd )
{
    //不同品牌的SDO又不一样,即使是60开头的.
    int vendor = ecatDevs[nd].devInfo.vendorId;
    //    switch( vendor )
    //    {
    //    case D_VENDOR_ID_TCR:
    //        break;
    //    case D_VENDOR_ID_Delta:
    //        break;
    //    case D_VENDOR_ID_Sanyo:
    //        break;
    //    case D_VENDOR_ID_LS:
    //        break;
    //    case D_VENDOR_ID_Panasonic:
    //        break;
    //    case D_VENDOR_ID_Invt:
    //        break;
    //    default:
    //        break;
    //    }

    bool ok = false;
    int cont = 0;       //SDO下载返回值
    unsigned int err;       //SDO错误类型
    quint8 sdoDataBuf[ sizeof(unsigned int) ];
    memset(sdoDataBuf, 0, sizeof(sdoDataBuf));

    followingErrorData[nd]=10000000;
    uint32_to_bytes( followingErrorData[nd], sdoDataBuf );
    startHomeTime[nd] = time( &startHomeTime[nd] );
    if( ( cont = ecrt_master_sdo_download(master, nd, 0x6065,0,sdoDataBuf, 4,&err) ) < 0 )
    {
        usleep(100000);
        endHomeTime[nd] = time(NULL);
        if( endHomeTime[nd]-startHomeTime[nd] > 0)
        {
            //qDebug()<< "配置跟随误差错误：超过跟随误差范围限制!";
            sdoError = 1<<nd;
            //break;
        }
    }

#if 1

    //if( vendor==D_VENDOR_ID_Sanyo )
    {
        //运行模式
        ampMode[nd]=E_ServoModeCsp;
        startHomeTime[nd] = time( &startHomeTime[nd] );
        if( ( cont = ecrt_master_sdo_download(master, nd, 0x6060,0, &ampMode[nd] ,1,&err) ) < 0 )
        {
            usleep(10000);
            endHomeTime[nd] = time(NULL);
            if( endHomeTime[nd]-startHomeTime[nd] > 0)
            {
                //			rt_printf("__%02d__6060time_out_err__\n", nd);
                sdoError = 1<<nd;
                //break;
            }
        }
    }
   // qDebug()<<"========sdo x1  "<<vendor;

    if( vendor==D_VENDOR_ID_Invt
            || vendor==D_VENDOR_ID_Delta)
        return;//台达，英威腾很多SDO不支持

    //设置0x605a,quick stop的设置
    u_int16_t u16QuickStop;
    u16QuickStop = 2;          //停止选项
    uint16_to_bytes( u16QuickStop, sdoDataBuf );
    startHomeTime[nd] = time( &startHomeTime[nd] );
    if( ( cont = ecrt_master_sdo_download(master,nd, 0x605a,0,sdoDataBuf,2,&err) ) < 0 )
    {
        usleep(100000);
        endHomeTime[nd] = time(NULL);
        if( endHomeTime[nd]-startHomeTime[nd]>0 )
        {
            rt_printf("set sdo error: 0x605a : % ", nd);
            sdoError = 1<<nd;
            //break;
        }
    }

  //  qDebug()<<"========sdo x2  ";
    //设置0x6085,quick stop的减速度
    uint32_to_bytes( 4000000000, sdoDataBuf);//u32 max
    startHomeTime[nd] = time( &startHomeTime[nd] );
    if( ( cont = ecrt_master_sdo_download(master,nd, 0x6085,0,sdoDataBuf,4,&err) ) < 0 )
    {
        usleep(100000);
        endHomeTime[nd] = time(NULL);
        if( endHomeTime[nd]-startHomeTime[nd] > 0)
        {
            rt_printf("set sdo error: 0x6085 : % ", nd);
            sdoError = 1<<nd;
            //break;
        }
    }


  //  qDebug()<<"========sdo x3  ";
    int16_t modewr = 0;          //shutdown 减速停止选项   使用0=0x3506 ,1=0x6084, 三洋报错

    if( vendor==D_VENDOR_ID_Panasonic ){
        uint16_to_bytes( modewr, sdoDataBuf );
        startHomeTime[nd] = time( &startHomeTime[nd] );
        if( ( cont = ecrt_master_sdo_download(master, nd, 0x605b, 0,sdoDataBuf,2,&err) ) < 0 )
        {
            usleep(100000);
            endHomeTime[nd] = time(NULL);
            if( endHomeTime[nd]-startHomeTime[nd] > 0)
            {
                rt_printf("set sdo error: 0x605b : % ", nd);
                sdoError = 1<<nd;
              //  break;
            }
        }

        modewr = 2;          //  fault （错误） 减速停止选项   使用 0=0x3510 , 1=0x6084, 2=0x6085 ,三洋报错
        uint16_to_bytes( modewr, sdoDataBuf );
        startHomeTime[nd] = time( &startHomeTime[nd] );
        if( ( cont = ecrt_master_sdo_download(master, nd, 0x605e,0,sdoDataBuf,2,&err) ) < 0 )
        {
            usleep(100000);
            endHomeTime[nd] = time(NULL);
            if( endHomeTime[nd]-startHomeTime[nd] > 0)
            {
                rt_printf("set sdo error: 0x605e : % ", nd);
                sdoError = 1<< nd;
           //     break;
            }
        }
    }

  //  qDebug()<<"========sdo x4  ";

    modewr = 0;          //op disable 减速停止选项   使用0=0x3506 ,1=0x6084
    uint16_to_bytes( modewr, sdoDataBuf );
    startHomeTime[nd] = time( &startHomeTime[nd] );
    if( ( cont = ecrt_master_sdo_download(master, nd, 0x605c, 0,sdoDataBuf,2,&err) ) < 0 )
    {
        usleep(100000);
        endHomeTime[nd] = time(NULL);
        if( endHomeTime[nd]-startHomeTime[nd] > 0)
        {
            rt_printf("set sdo error: 0x605c : % ", nd);
            sdoError = 1<<nd;
          //  break;
        }
    }


  //  qDebug()<<"========sdo x5  ";

    int halt_data = 2;          //halt（内部暂停指令） 减速停止选项   使用 1=0x6084 , 2=0x6085, 3=0x6072
    uint16_to_bytes( halt_data, sdoDataBuf );
    startHomeTime[nd] = time( &startHomeTime[nd] );
    if( ( cont = ecrt_master_sdo_download(master, nd, 0x605d, 0, sdoDataBuf,2,&err) ) < 0 )
    {
        usleep(100000);
        endHomeTime[nd] = time(NULL);
        if( endHomeTime[nd]-startHomeTime[nd] > 0)
        {
            rt_printf("set sdo error: 0x605d : % ", nd);
            sdoError = 1<<nd;
          //  break;
        }
    }

   // qDebug()<<"========sdo x6  ";

    /*
    ///////////LS////////
    //servo-off时序
    u_int16_t servoOffSequence;
    servoOffSequence = 0;          //停止选项
    uint16_to_bytes( servoOffSequence, sdoDataBuf );
    startHomeTime[nd] = time( &startHomeTime[nd] );
    while( ( cont = ecrt_master_sdo_download(master,nd, 0x3506,0,sdoDataBuf,2,&err) ) < 0 )
    {
        usleep(100000);
       endHomeTime[nd] = time(NULL);
         if( endHomeTime[nd]-startHomeTime[nd] > 1)
        {
            //                rt_printf("__%02d__605a_time_out_err__\n", nd);
            sdoError = 1<<nd;
            break;
        }
    }


    //设置0x200d,设置.  伺服锁定+伺服SW正转
        if( 0 == nd )
        {
            uint16_to_bytes(0x02, sdoDataBuf);
        }
        else
        {

            uint16_to_bytes(0x03, sdoDataBuf);
        }
        uint16_to_bytes(0x03, sdoDataBuf);//伺服锁定+伺服SW正转
        startHomeTime[nd] = time( &startHomeTime[nd] );
        while( ( cont = ecrt_master_sdo_download(master,nd, 0x200D,0,sdoDataBuf,2,&err) ) < 0 )
        {
            endHomeTime[nd] = time(NULL);
            if( endHomeTime[nd]-startHomeTime[nd] > 3)
            {
                //            rt_printf("__%02d__0x200D_0_time_out_err__\n", nd);
                sdoError = 1<<nd;
                break;
            }
        }

    */
#endif
    // 0x607e  SX  旋转方向
    // 0x3015  SX  1是相对，  0x7 0，2是绝对
    // 0x605a  SX  快速停止减速度
    // 0x605d  SX  错误发生减速方式
}


bool CoeEcatDevice::customErrorSearch()
{
    if( (etherCatCommError
         |sdoError
         |aliasMatchError
         |slaveConfigError
         |domainStateRrror)!=0 )
    {
        // qDebug()<< "==ethercat Error== :"<<etherCatCommError<<sdoError<<aliasMatchError<<slaveConfigError<<domainStateRrror;
        return true;
    }else{
        return false;
    }

}


// 函数功能：SDO资源申请 0x607c,LS-HomeOffset 原点偏移
bool CoeEcatDevice::sdoHomeOffsetRequest(ec_slave_config_t *sc, int node)
{
    pHomeOffsetSdoRequest[node] = ecrt_slave_config_create_sdo_request(sc, 0x607c, 0x0, 4);
    if( NULL == pHomeOffsetSdoRequest[node] )
    {
        rt_printf(" SX alarm code read error: sdo Request fail , -1! \n");
        return false;
    }
    ecrt_sdo_request_timeout(pHomeOffsetSdoRequest[node], 3000);
    return true;
}


bool CoeEcatDevice::regRealtimeRead( ec_reg_request_t *pRegRequest,
                                     int pos, void *val , int byteNum, int regadr )
{
    Q_UNUSED(pos)
    switch( ecrt_reg_request_state( pRegRequest ))
    {
    case EC_REQUEST_UNUSED:
        ecrt_reg_request_read( pRegRequest, regadr, byteNum );
        return false;
    case EC_REQUEST_BUSY:
        //rt_printf("Still Read busy...\n");
        return false;
    case EC_REQUEST_SUCCESS:
    {
        switch( byteNum )
        {
        case 1:
        {
            *(quint8 *)val = EC_READ_U8( ecrt_reg_request_data( pRegRequest ));
        }
            break;
        case 2:
        {
            *(unsigned short *)val = EC_READ_U16( ecrt_reg_request_data( pRegRequest ) );
        }
            break;
        case 4:
        {
            *(unsigned int *)val = EC_READ_U32( ecrt_reg_request_data( pRegRequest ) );
        }
            break;
        case 8:
        {
            *(uint64_t *)val = EC_READ_U64( ecrt_reg_request_data( pRegRequest ) + 7);
        }
            break;
        default:
            break;
        }
        ecrt_reg_request_read( pRegRequest, regadr, byteNum );
    }
        return true;
    case EC_REQUEST_ERROR:
        //rt_printf( "fail to read reg\n" );
        ecrt_reg_request_read( pRegRequest, regadr, byteNum );   //重复写
        return false;
    }
    return true;
}


bool CoeEcatDevice::regRealtimeWrite( ec_reg_request_t *pRegRequest, int pos, void *val , int byteNum, int regadr )
{
    Q_UNUSED(pos)
    switch( ecrt_reg_request_state( pRegRequest ) )
    {
    case EC_REQUEST_UNUSED:
        ecrt_reg_request_write( pRegRequest, regadr, byteNum );
        return false;
    case EC_REQUEST_BUSY:
        //rt_printf("Still Read busy...\n");
        return false;
    case EC_REQUEST_SUCCESS:
    {
        switch( byteNum )
        {
        case 1:
        {
            EC_WRITE_U8( ecrt_reg_request_data( pRegRequest ) , *(quint8 *)val);
        }
            break;
        case 2:
        {
            EC_WRITE_U16( ecrt_reg_request_data( pRegRequest ), *(unsigned short *)val);
        }
            break;
        case 4:
        {
            EC_WRITE_U32( ecrt_reg_request_data( pRegRequest ), *(unsigned int *)val);
        }
            break;
        case 8:
        {
            EC_WRITE_U64( ecrt_reg_request_data( pRegRequest ), *(uint64_t *)val);
        }
            break;
        default:
            break;
        }
        ecrt_reg_request_write( pRegRequest, regadr, byteNum );
    }
        rt_printf( "..\n" );
        return true;
    case EC_REQUEST_ERROR:
        //rt_printf( "fail to read reg\n" );
        ecrt_reg_request_write( pRegRequest, regadr, byteNum );   //重复写
        return false;
    }
    return true;
}

//申请寄存器资源:别名
bool CoeEcatDevice::regAliasSetRequestRealtime( ec_slave_config_t *sc, int node )
{
    pAliasSetRegRequest[node] = ecrt_slave_config_create_reg_request(sc,2);
    if( !pAliasSetRegRequest[node] )
    {
        rt_printf("pAliasSetRegRequest reg Request fail !");
        return false;
    }
    return true;
}


bool CoeEcatDevice::sdoRealtimeRead( ec_sdo_request_t *pSdoRequest,
                                     int pos, void *val , int byteNum, int offset )
{
    Q_UNUSED(pos)
    switch( ecrt_sdo_request_state( pSdoRequest ))
    {
    case EC_REQUEST_UNUSED:
        ecrt_sdo_request_read( pSdoRequest );
        return false;
    case EC_REQUEST_BUSY:
        //rt_printf("Still Read busy...\n");
        return false;
    case EC_REQUEST_SUCCESS:
    {
        switch( byteNum )
        {
        case 1:
        {
            quint8 *tmp = (quint8 *)val;
            *tmp = EC_READ_U8( ecrt_sdo_request_data( pSdoRequest ) + offset);
        }
            break;
        case 2:
        {
            unsigned short *tmp1 = (unsigned short *)val;
            *tmp1 = EC_READ_U16( ecrt_sdo_request_data( pSdoRequest ) + offset);
        }
            break;
        case 4:
        {
            unsigned int *tmp2 = (unsigned int *)val;
            *tmp2 = EC_READ_U32( ecrt_sdo_request_data( pSdoRequest ) + offset);
        }
            break;
        default:
            break;
        }
        ecrt_sdo_request_read( pSdoRequest );
    }
        return true;
    case EC_REQUEST_ERROR:
        //            rt_printf( "fail to read sdo\n" );
        ecrt_sdo_request_read( pSdoRequest );   //重复写
        return false;
    }
    return true;
}

bool CoeEcatDevice::sdoRealtimeWrite( ec_sdo_request_t *pSdoRequest, int pos,  int v , int byteNum, int offset )
{
    Q_UNUSED(byteNum)
    Q_UNUSED(pos)
    switch( ecrt_sdo_request_state( pSdoRequest ))
    {
    case EC_REQUEST_UNUSED:
        ecrt_sdo_request_write( pSdoRequest );
        return false;
    case EC_REQUEST_BUSY:
        //rt_printf("Still Read busy...\n");
        return false;
    case EC_REQUEST_SUCCESS:
        EC_WRITE_U32( ecrt_sdo_request_data( pSdoRequest ) + offset, (unsigned int)v );
        ecrt_sdo_request_write( pSdoRequest );
        //rt_printf( "success to write sdo\n" );
        return true;
    case EC_REQUEST_ERROR:
        //rt_printf( "fail to write sdo\n" );
        ecrt_sdo_request_write( pSdoRequest );   //重复写
        return false;
    }
    return true;
}


//写别名
int CoeEcatDevice::setAliasRobotAxis(int robID, int axis, unsigned short alias )
{
    if( robID<0  )
    {
        qDebug()<<"setAliasRobotAxis: robot ID can not be -1 !";
        return 0;
    }else{
        QVector<int> motorAlias = robots.value(robID).motorAlias;
        int nd = aliasMapNode.value( motorAlias.at(axis) );
        return setAlias(nd, alias);
    }
}

int CoeEcatDevice::setAlias(int node , unsigned short alias)
{
    char *aliasCmd = NULL;
    QString writeAliasCmd;
    QString pos = QString::number( node );
    QString a = QString::number( alias );

    //    int32_to_bytes(node, pos);
    //    int16_to_bytes(alias, a);
    //sprintf( aliasCmd, "ethercat alias -p %d %d", node, alias );

    writeAliasCmd.push_back("ethercat alias -p ");
    writeAliasCmd.push_back(pos);
    writeAliasCmd.push_back(" ");
    writeAliasCmd.push_back(a);

    qDebug()<<"___"<<writeAliasCmd<<"___"<<endl;

    QByteArray buf = writeAliasCmd.toLatin1();
    aliasCmd = buf.data();

    system( aliasCmd );

    return 1;
}




int CoeEcatDevice::robotAxisToEthercatNode( int robID, int axis, int &returnNode )
{
    QVector<int> motorAlias = robots.value(robID).motorAlias;
    if( motorAlias.size()>0 )
    {
        if( aliasMapNode.contains(motorAlias.at(axis)) )
        {
            returnNode = aliasMapNode.value( motorAlias.at(axis) );
            return 1;
        }
    }
    return 0;
}

#include <stdio.h>
// vector<string> &resvec
int CoeEcatDevice::getResultFromSystemCall(const char* pCmd, char* pResult, int size)
{

#if 0
    //命令行返回为空时无法.
    int fd[2];
    pipe(fd);
    fflush(stdout);

    int bak_fd = dup(STDOUT_FILENO);
    int new_fd = dup2(fd[1], STDOUT_FILENO);

#if 0
    QByteArray buf(pCmd);
    if(buf.isEmpty() )
    {
        qDebug()<<"cmd is empty!";
        return 0;
    }
    system( buf.constData() );
#else
    system( pCmd );
#endif

    read(fd[0], pResult, size);
    pResult[strlen(pResult) -1] = 0;
    dup2(bak_fd,new_fd);

#else
    FILE *fstream=NULL;
    if(NULL==(fstream=popen( pCmd  ,"r")))
    {
        return -1;
    }

    while(NULL!=fgets(pResult, size, fstream))
    {
        ;//fstream >> ;//printf("%s",buff);
    }

    pclose(fstream);
#endif

    return 0;
}


void CoeEcatDevice::uint32_to_bytes(unsigned int i, quint8 *b)
{
    b[0] = ByteCast(i);
    b[1] = ByteCast(i>>8);
    b[2] = ByteCast(i>>16);
    b[3] = ByteCast(i>>24);
}

void CoeEcatDevice::uint16_to_bytes(unsigned short i, quint8 *b)
{
    b[0] = ByteCast(i);
    b[1] = ByteCast(i>>8);
}

unsigned short CoeEcatDevice::bytes_to_uint16( quint8 *b )
{
    unsigned short ret;
    ret = (unsigned short)(ByteCast(b[0]));
    ret |= (unsigned short)(ByteCast(b[1])) << 8;
    return ret;
}

bool CoeEcatDevice::isAiAo( int node, unsigned int index , unsigned int subindex  )
{
    bool ok=false;
    for(int i=0;i<AiAoObjectInfo.size();i++)
    {
        if(index!=AiAoObjectInfo[i].index )
        {
            continue;
        }
        if(subindex!=AiAoObjectInfo[i].subindex )
        {
            continue;
        }
        if(node!=AiAoObjectInfo[i].node )
        {
            continue;
        }
        ok=true;
    }
    return ok;
}

void CoeEcatDevice::entriesConfig(int nd )
{
    int tmpAlist= nodeMapAlias.value(nd);
    S_EcatDevInfo* robDev=robotDevs.value( tmpAlist ,NULL);

    if(robDev==NULL )
    {
        qDebug()<<"robDev NULL Err:"<<__LINE__;
    }

    switch( robDev->devInfo.devType )
    {
    case E_EcatDev_Servo:
        for(int j=0;j<robDev->objConfig.ec_syncs.size(); j++)
        {
            for(int k=0;k<robDev->objConfig.ec_syncs.at(j).n_pdos;k++)
            {

                if((robDev->objConfig.ec_syncs.at(j).pdos+k)==NULL)
                    continue;

                for(int n=0;n<robDev->objConfig.ec_syncs.at(j).pdos[k].n_entries;n++)
                {
                    if( (robDev->objConfig.ec_syncs.at(j).pdos[k].entries+n) ==NULL )
                        continue;
                    {//entryNum
                        if( D_MAP_OBJECT_CONTROL_WORD == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[ nd ].offsetControlWorldSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetControlWorldSet;
                        }
                        else if( D_MAP_OBJECT_TARGET_POSITION == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[ nd ].offsetTargetPosSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetTargetPosSet;
                        }
                        else if( D_MAP_OBJECT_OPERATION_MODE == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[ nd ].offsetAmpModeSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetAmpModeSet;
                        }
                        else if( D_MAP_OBJECT_STATUS_WORD == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[ nd ].offsetServoStateGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetServoStateGet;
                        }
                        else if( D_MAP_OBJECT_POSITION_ACTUAL == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[ nd ].offsetEncoderGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetEncoderGet;
                        }
                        else if( D_MAP_OBJECT_TORQUE_ACTUAL == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[nd].offsetTorqueGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetTorqueGet;
                        }
                        else if( D_MAP_OBJECT_DIGITAL_INPUT == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[nd].offsetMotorDiGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetMotorDiGet;
                        }
                        else if( D_MAP_OBJECT_VELOCITY_ACTUAL == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[nd].offsetSpeedGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetSpeedGet;
                        }
                        else if( D_MAP_OBJECT_MODES_OF_OPERATION_DISPLAY == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[nd].offsetAmpModeGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetAmpModeGet;
                        }
                        else if( D_MAP_OBJECT_ERROR_CODE_603F == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index
                                 || D_MAP_OBJECT_ERROR_CODE_sanyo == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index)
                        {
                            ecOffOd[nd].offsetErrorCodeGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetErrorCodeGet;
                        }

                        else if( D_MAP_OBJECT_FOLLOWINGERROR_ACTUAL == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[nd].offsetFollowingErrorGet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetFollowingErrorGet;
                        }
                        else if( D_MAP_OBJECT_HomeMeth == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            ecOffOd[nd].offsetHomeMethodSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetHomeMethodSet;
                        }
                        else if( D_MAP_OBJECT_HomeSearchSwitch == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {
                            if( 0x01==robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].subindex )
                            {
                                ecOffOd[nd].offsetSpeedSearchSwitchSet = domainOffKeep/8;
                                builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetSpeedSearchSwitchSet;
                            }else if( 0x02==robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].subindex )
                            {
                                ecOffOd[nd].offsetSpeedSearchZeroSet = domainOffKeep/8;
                                builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetSpeedSearchZeroSet;
                            }
                        }
                        else if( D_MAP_OBJECT_HomeAcc == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {//回零加速度
                            ecOffOd[nd].offsetHomeAccSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetHomeAccSet;
                        }
                        else if( D_MAP_OBJECT_VELOCITY_OFFSET == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {//写 速度漂移
                            ecOffOd[nd].offsetSpeedOffSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetSpeedOffSet;
                        }
                        else if( D_MAP_OBJECT_TORQUE_OFFSET == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {//写扭矩漂移
                            ecOffOd[nd].offsetTorqueOffSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetTorqueOffSet;
                        }
                        else if( D_MAP_OBJECT_TARGET_TORQUE == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {//写目标扭矩
                            ecOffOd[nd].offsetTargetTorqueSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetTargetTorqueSet;
                        }
                        else if( D_MAP_OBJECT_TARGET_SPEED == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        { //写目标速度
                            ecOffOd[nd].offsetTargetSpeedSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetTargetSpeedSet;

                        }
                        else if( D_MAP_OBJECT_NULL_8BIT == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        { //假字节
                            ecOffOd[nd].offsetNullData8bitSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetNullData8bitSet;
                        }

                        /*  else if( D_MAP_OBJECT_ＭAX_SPEED == robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index )
                        {//最大速度
                            ecOffOd[nd].offsetMaxSpeedSet = domainOffKeep/8;
                            builddomainRegs[ offCntKeep ].offset = &ecOffOd[nd].offsetMaxSpeedSet;

                        }*/

                        builddomainRegs[ offCntKeep ].alias = 0;// robDev->devInfo.alias;
                        builddomainRegs[ offCntKeep ].position = nd;
                        builddomainRegs[ offCntKeep ].vendor_id = robDev->devInfo.vendorId;
                        builddomainRegs[ offCntKeep ].product_code = robDev->devInfo.productCode;
                        builddomainRegs[ offCntKeep ].index = robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index;
                        builddomainRegs[ offCntKeep ].subindex = robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].subindex;
                        builddomainRegs[ offCntKeep ].bit_position = NULL;

                        domainOffKeep += robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].bit_length;
                        offCntKeep++;
                        //   ecOffOd[nd].offCnt++;
                    }
                }
            }
        }

        break;//   motor

    case E_EcatDev_Coupler:
        break;//

    case E_EcatDev_IoModule:
    {
        int diCounter = 0;
        int doCounter = 0;
        int aiCounter = 0;
        int aoCounter = 0;

    //    qDebug()<< "362563"<< robDev->objConfig.ec_entries.size()  ;
        ioDataOffsetList[nd].ioDi.resize( robDev->objConfig.ec_entries.size() );
        ioDataOffsetList[nd].ioDo.resize( robDev->objConfig.ec_entries.size() );
        ioDataOffsetList[nd].ioAi.resize( robDev->objConfig.ec_entries.size() );
        ioDataOffsetList[nd].ioAo.resize( robDev->objConfig.ec_entries.size() );

        for(int j=0;j<robDev->objConfig.ec_syncs.size();j++)
        {
            if( robDev->objConfig.ec_syncs.at(j).dir == EC_DIR_INPUT )
            {
                for(int k=0;k<robDev->objConfig.ec_syncs.at(j).n_pdos;k++)
                {
                    for(int n=0;n<robDev->objConfig.ec_syncs.at(j).pdos[k].n_entries;n++)
                    {

                        builddomainRegs[ offCntKeep ].alias = 0;// robDev->devInfo.alias;
                        builddomainRegs[ offCntKeep ].position = nd;
                        builddomainRegs[ offCntKeep ].vendor_id = robDev->devInfo.vendorId;
                        builddomainRegs[ offCntKeep ].product_code = robDev->devInfo.productCode;
                        builddomainRegs[ offCntKeep ].index = robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index;
                        builddomainRegs[ offCntKeep ].subindex = robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].subindex;

                        if( isAiAo( nd,   builddomainRegs[ offCntKeep ].index,    builddomainRegs[ offCntKeep ].subindex ) )
                        {
                            builddomainRegs[ offCntKeep ].offset = &( ioDataOffsetList[nd].ioAi[aiCounter].byte_offset );
                            builddomainRegs[ offCntKeep ].bit_position = &( ioDataOffsetList[nd].ioAi[aiCounter].bit_offset );
                            aiCount++;
                            aiCounter++;
                        }else{
                            builddomainRegs[ offCntKeep ].offset = &( ioDataOffsetList[nd].ioDi[diCounter].byte_offset );
                            builddomainRegs[ offCntKeep ].bit_position = &( ioDataOffsetList[nd].ioDi[diCounter].bit_offset );
                            diCount++;
                            diCounter++;
                        }

                        offCntKeep++;
                        domainOffKeep +=  robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].bit_length;
                    }
                }
            }
            else if( robDev->objConfig.ec_syncs.at(j).dir == EC_DIR_OUTPUT )
            {
                for(int k=0;k<robDev->objConfig.ec_syncs.at(j).n_pdos;k++)
                {
                    for(int n=0;n<robDev->objConfig.ec_syncs.at(j).pdos[k].n_entries;n++)
                    {

                        builddomainRegs[ offCntKeep ].alias = 0;// robDev->devInfo.alias;
                        builddomainRegs[ offCntKeep ].position = nd;
                        builddomainRegs[ offCntKeep ].vendor_id = robDev->devInfo.vendorId;
                        builddomainRegs[ offCntKeep ].product_code = robDev->devInfo.productCode;
                        builddomainRegs[ offCntKeep ].index = robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].index;
                        builddomainRegs[ offCntKeep ].subindex = robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].subindex;

                        if( isAiAo( nd,   builddomainRegs[ offCntKeep ].index,    builddomainRegs[ offCntKeep ].subindex ) )
                        {
                            builddomainRegs[ offCntKeep ].offset = &( ioDataOffsetList[nd].ioAo[aoCounter].byte_offset );
                            builddomainRegs[ offCntKeep ].bit_position = &( ioDataOffsetList[nd].ioAo[aoCounter].bit_offset );
                            aoCount++;
                            aoCounter++;
                        }else{
                            builddomainRegs[ offCntKeep ].offset = &( ioDataOffsetList[nd].ioDo[doCounter].byte_offset );
                            builddomainRegs[ offCntKeep ].bit_position = &( ioDataOffsetList[nd].ioDo[doCounter].bit_offset );
                            doCount++;
                            doCounter++;
                        }
                        offCntKeep++;
                        domainOffKeep += robDev->objConfig.ec_syncs.at(j).pdos[k].entries[n].bit_length;

                    }
                }
            }
        }
        ioDeviceDataOffsetMap.insert( tmpAlist, &(ioDataOffsetList[nd]) );

        int ioDataSize=ioDeviceData.size();
        ioDeviceData.resize(ioDataSize+1);
        ioDeviceData[ioDataSize].ioAo.resize(aoCounter);
        ioDeviceData[ioDataSize].ioAi.resize(aiCounter);
        ioDeviceData[ioDataSize].ioDi.resize(diCounter);
        ioDeviceData[ioDataSize].ioDo.resize(doCounter);
        ioDeviceData[ioDataSize].nodeId  = robDev->devInfo.alias;

        qDebug()<<"ge4564 io设备:"<<nd<<ioDataSize <<"，物理口："
               <<diCounter<<doCounter<<aiCounter<<aoCounter;
    }
        break; // io

    case E_EcatDev_TypeErr:
        break;
    default:
        break;
    }

}

void CoeEcatDevice::syncsConfig(ec_slave_config_t *&sc, int nd )
{
    int siz=ecatDevs[nd].objConfig.ec_syncs.size();
    if( ecrt_slave_config_pdos( sc, siz,  &(ecatDevs[nd].objConfig.ec_syncs[0]) ) )
    {
        rt_printf( "fail to configure syncs.\n");
        slaveConfigError |= 1<<nd;
    }

#if D_USE_DC_SYNC
    //配置同步管理时钟
   if( ecatDevs[nd].devInfo.devType==E_EcatDev_Servo ){
        ecrt_slave_config_dc(sc, 0x300, cycle_ns , 0, 0, 0);//开启
#if D_IF_DEBUG
        rt_printf("开启DC同步,站号: %d\n", nd);
#endif
    }else{
        ecrt_slave_config_dc(sc, 0x0, cycle_ns , 0, 0, 0);//off
    }
#else
    if(ecatDevs[nd].devInfo.devType==E_EcatDev_Servo ){
        ecrt_slave_config_dc(sc, 0x0, cycle_ns , 4400000, 0, 0);//关闭
    }
#endif
}

/*
void readData( )
            {

}
void writeData( )
            {

}*/


void CoeEcatDevice::updateDomainData()
{

    //   if( 0 ==  pthread_mutex_trylock(&ecatThreadMutex) )
    {
        pthread_mutex_lock(&ecatThreadMutex) ;
        for(int j=0; j<encoderValue.size(); j++ )
        {
            if( motorCount<1 )
                break;

            int nd =j;
            encoderValueOld[nd] = encoderValue[nd];
            if( ecOffOd[nd].offsetServoStateGet  )
            {
                staWord[nd] = EC_READ_S16(domain_p + ecOffOd[nd].offsetServoStateGet );
            }
            if( ecOffOd[nd].offsetEncoderGet  )
            {
                realEncoderValue[nd]=EC_READ_S32(domain_p + ecOffOd[nd].offsetEncoderGet);
                encoderValue[nd] = realEncoderValue[nd] - updateHomeOffset[nd] ;
            }
            if( ecOffOd[nd].offsetSpeedGet  )
            {
                velocityActValue[nd] = EC_READ_S32(domain_p + ecOffOd[nd].offsetSpeedGet);
            }
            if( ecOffOd[nd].offsetTorqueGet  )
            {
                torqueActValue[nd] = EC_READ_S16(domain_p + ecOffOd[nd].offsetTorqueGet);
            }
            if( ecOffOd[nd].offsetMotorDiGet  )
            {
                motorDi[nd] = EC_READ_U32( domain_p + ecOffOd[nd].offsetMotorDiGet );
            }
            if( ecOffOd[nd].offsetFollowingErrorGet  )
            {
                followingErrorActualValue[nd] = EC_READ_U32(domain_p + ecOffOd[nd].offsetFollowingErrorGet);
            }
            if( ecOffOd[nd].offsetAmpModeGet  )
            {
                ampModeDisp[nd] = EC_READ_U8( domain_p + ecOffOd[nd].offsetAmpModeGet );
            }
            if( ecOffOd[nd].offsetErrorCodeGet )
            {
                errorCode[nd] = EC_READ_U16( domain_p + ecOffOd[nd].offsetErrorCodeGet );
            }
            if( ecOffOd[nd].offsetMaxSpeedSet  )
            {
                maxSpeed[nd] = EC_READ_S32( domain_p + ecOffOd[nd].offsetMaxSpeedSet );
            }
            if( ecOffOd[nd].offsetTargetSpeedSet   )
            {
                targetSpeed[nd] = EC_READ_S32( domain_p + ecOffOd[nd].offsetTargetSpeedSet );
            }


#if 1
            //第一个数据是0开始的,所以ecOffOd[0].offsetControlWorldSet＝0
            if( ( nd != 0 )  && (ecOffOd[nd].offsetControlWorldSet) )
                EC_WRITE_U16(domain_p + ecOffOd[nd].offsetControlWorldSet, ctlWord[nd] );
            else if( nd == 0)
                EC_WRITE_U16(domain_p + ecOffOd[nd].offsetControlWorldSet, ctlWord[nd] );
#else
            //这个写法是错误的
            if( (ecOffOd[nd].offsetControlWorldSet) )
                EC_WRITE_U16(domain_p + ecOffOd[nd].offsetControlWorldSet, ctlWord[nd] );
#endif

            //目标位置
            if( ecOffOd[nd].offsetTargetPosSet )
                EC_WRITE_S32(domain_p + ecOffOd[nd].offsetTargetPosSet,  targetPos[nd]  + updateHomeOffset[nd] ) ;
            //目标扭矩
            if( ecOffOd[nd].offsetTargetTorqueSet )
                EC_WRITE_S16(domain_p + ecOffOd[nd].offsetTargetTorqueSet, targetTorque[nd]);
            //写目标速度
            if( ecOffOd[nd].offsetTargetSpeedSet )
                EC_WRITE_S16(domain_p + ecOffOd[nd].offsetTargetSpeedSet, targetSpeed[nd]);
            //扭矩漂移
            if( ecOffOd[nd].offsetTorqueOffSet )
                EC_WRITE_S16(domain_p + ecOffOd[nd].offsetTorqueOffSet, torqueOffset[nd]);

            if( ecOffOd[nd].offsetAmpModeSet )
                EC_WRITE_S8(domain_p + ecOffOd[nd].offsetAmpModeSet, ampMode[nd] );

            if( ecOffOd[nd].offsetHomeMethodSet )
                EC_WRITE_S8(domain_p + ecOffOd[nd].offsetHomeMethodSet, homeMethod[nd] );
            if( ecOffOd[nd].offsetSpeedSearchZeroSet )
                EC_WRITE_S32(domain_p + ecOffOd[nd].offsetSpeedSearchZeroSet, homeSpeedToSwitch[nd] );
            if( ecOffOd[nd].offsetSpeedSearchSwitchSet )
                EC_WRITE_S32(domain_p + ecOffOd[nd].offsetSpeedSearchSwitchSet, homeSpeedToZ[nd] );
            if( ecOffOd[nd].offsetHomeAccSet )
                EC_WRITE_S32(domain_p + ecOffOd[nd].offsetHomeAccSet, homingAcc[nd] );

        }


        if( cycle_counter%D_IO_RW_TIME==0 )
        {
            for(int i=0; i<ioDeviceData.size(); i++)
            {
                EcatIoDeviceDataOffsetList* pData= ioDeviceDataOffsetMap.value( ioDeviceData[i].nodeId );
                if(pData!=NULL )
                {
                    for(int index=0; index<ioDeviceData[i].ioDi.size(); index++)
                    {
                        ioDeviceData[i].ioDi[index]  = EC_READ_BIT(  domain_p + pData->ioDi[index].byte_offset, pData->ioDi[index].bit_offset);
                    }

                    for(int index=0; index<ioDeviceData[i].ioDo.size(); index++)
                    {
                        EC_WRITE_BIT(  domain_p + pData->ioDo[index].byte_offset, pData->ioDo[index].bit_offset, ioDeviceData[i].ioDo[index] );
                    }

                    for(int index=0; index<ioDeviceData[i].ioAi.size(); index++)
                    {
                        ioDeviceData[i].ioAi[index]  = EC_READ_S16(  domain_p + pData->ioAi[index].byte_offset );
                    }
                    for(int index=0; index<ioDeviceData[i].ioAo.size(); index++)
                    {
                        EC_WRITE_S16(  domain_p + pData->ioAo[index].byte_offset, ioDeviceData[i].ioAo[index] );
                    }
                }
            }
        }
        pthread_mutex_unlock(&ecatThreadMutex) ;
    }
}



int CoeEcatDevice::rt_check_slaves_state()
{
    bool isok=true;
    for( int i = 0; i < robotUseAliasList.size(); i++ )
    {
        //获取从站信息
        int node =  aliasMapNode.value(robotUseAliasList.at(i), -1);
        if( node<0 || !slave_sc[node] ){
            continue;
        }
        if( isOp[node] ){
            continue;
        }else{
            isok=false;
        }
        // 调用ecrt_master_get_slave 会导致死机?
  //      ec_slave_info_t slave_info;
//        int ret =  ecrt_master_get_slave( master, node, &slave_info  );
//        if(ret){
//            rt_printf("w5234 獲取失敗==========\n");
//            continue;
//        }

        ec_slave_config_state_t slave_state;
        ecrt_slave_config_state( slave_sc[node], &slave_state ) ;
        unsigned int al_state= slave_state.al_state;//
        int __error_flag=0;
        if( al_state &0x0E && al_state &0x01 ){
            qDebug()<<"有错误标志";
            if( --error_flag_count <1 ){
                __error_flag=1;
            }
        }

        char* errstr="";
        if( __error_flag ){
            errstr=" + ERROR";
            error_flag=1;
        }
        switch(  al_state &0x0E )
        {
         case 1:
             rt_printf( "Slave %d AL: INIT%s\n" , node, errstr );
             break;
         case 2:
            if( __error_flag )
                rt_printf( "Slave %d AL: PREOP%s\n " , node, errstr );
             break;
         case 4:
             rt_printf( "Slave %d AL: SAFEOP%s\n", node , errstr );
             break;
         case 8:
             rt_printf( "Slave %d AL: OP%s\n" , node, errstr );
             isOp[node] = true;
             break;
         default:
             rt_printf( "Slave %d AL: ???%s\n" , node, errstr );
             break;
         }
          if( __error_flag )
          {
              return -1;
          }
    }

    return isok?0:1;
}

int CoeEcatDevice::rt_check_master_state()
{
    ec_master_state_t ms;
    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding )
    {
        rt_printf("%u slave(s).\n", ms.slaves_responding);
    }

    if (ms.al_states != master_state.al_states )
    {
        rt_printf("Master AL states: 0x%02X.\n", ms.al_states);
    }

    if (ms.link_up != master_state.link_up )
    {
        rt_printf("Master link is %s.\n", ms.link_up ? "up" : "down");
    }

    if( false==ms.link_up )
    {
        rt_printf("39 ethercat 设备没有响应, 请检查网卡或网络状态！\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 39, -1  );
        return -1;
    }

    master_state = ms;
    return 0;
}

int CoeEcatDevice::rt_check_domain_state(void)
{
    int ret = 0;//ok
    ec_domain_state_t ds;
    //  memset( &ds, 0, sizeof( ec_domain_state_t ) );
    ecrt_domain_state(domain, &ds);

    // domainStateRrror=0;
    if( readdyOk )
    {
        if (ds.working_counter < robotDevs.size() || (ds.wc_state != domain_state.wc_state && EC_WC_COMPLETE!=ds.wc_state))
        {
            slave_not_read_counter++;
           // if (ds.wc_state != domain_state.wc_state && EC_WC_COMPLETE!=ds.wc_state )
            { 
                if(slave_not_read_counter>2)
                {
                    if( domainStateRrror ==0 ){
                        rt_printf("error : domain: State %d , %d , %d .\n", ds.wc_state, ds.working_counter, robotDevs.size() );
                        addMsg( ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 31, -1 , ds.wc_state, ds.working_counter, max_time );
                    }
                    domainStateRrror = 0x01;
                }
                ret |= 1<<2;
            }
            //            else{
            //                if(slave_not_read_counter>2)
            //                {
            //                    if( domainStateRrror==0 )
            //                    {
            //                        rt_printf("error :  domain: WC %u.\n", ds.working_counter);
            //                        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 30, -1 );
            //                    }
            //                    domainStateRrror = 0x01;
            //                }
            //                ret |= 1<<1;
            //            }
        }else{
            slave_not_read_counter=0;
            domain_state = ds;
        }

        if(domainStateRrror && !faultOccurred )
        {
            faultOccurred=true;
            domain_state = ds;
        }
    }else{
        domain_state = ds;
    }

    return ret;
}

void CoeEcatDevice::cycleStartTime(void)
{
//    isMasterActived=true;
//    clock_gettime(CLOCK_MONOTONIC , &wakeupTime);//
//    return;
    while( !isInitOk ){
        sleep_ns_real_time( 1000000 );
    }
#ifndef SIMULATION_MODE
    dc_start_time_ns = 0LL;
    dc_time_ns = 0;
  //  system_time_base = 0LL; //调整后的主站时钟？ //不重新赋值
#if SYNC_MASTER_TO_REF
    dc_started = 0;
    dc_diff_ns = 0; //主站 跟 参考时钟的 时间差
    prev_dc_diff_ns = 0;
    dc_diff_total_ns = 0LL;
    dc_delta_total_ns = 0LL;
    dc_filter_idx = 0;
    dc_adjust_ns;
#endif

#if D_USE_DC_SYNC

    //用寄存器读参考时钟，
    char retBuff[100]={0};
    char cmdBuf[100]={0};
    QString str;
    int timeout_count = 5;
    while( str.isEmpty() && timeout_count-- ){//出现空值str
        usleep(1000);
        memset(cmdBuf, 0, 100*sizeof(char) );
        memset(retBuff, 0, 100*sizeof(char) );
        sprintf(cmdBuf, "ethercat reg_read -p %d 0x910 -t uint64", 0 );
        getResultFromSystemCall( cmdBuf, retBuff, 100);
        str= QString( retBuff );
    }
    QStringList alist= str.split(" ");
    if( alist.size()==2){
       dc_start_time_ns =  alist.at(1).toULongLong();
    }

    //  uint32_t ref_time = 0;
    // ecrt_master_reference_clock_time(master, &ref_time);//获取 32位的参考时钟系统时间
    //  ecrt_master_sync_reference_clock(master); //将主站时钟覆盖参考时钟
    if( dc_start_time_ns>0 )  {
       // dc_start_time_ns = ref_time;
    }else{
       dc_start_time_ns = system_time_ns();
    }

   dc_start_time_ns +=NSEC_PER_SEC;//假设获取DC参考时钟消耗了1S
   quint64  app_time_base = system_time_ns();
   system_time_base += app_time_base-dc_start_time_ns;
  // app_time_base  = system_time_ns();

    dc_time_ns = dc_start_time_ns;
  //  ecrt_master_application_time(master, dc_start_time_ns+ NSEC_PER_SEC *6 );//设置主站时钟
    ecrt_master_application_time( master, dc_start_time_ns );//设置主站时钟
   // ecrt_master_sync_reference_clock(master); //将主站时钟覆盖参考时钟
    int ret=ecrt_master_select_reference_clock( master, NULL );//选择一个参考时钟,激活主站前设置

    qDebug()<<"选择时钟 " << dc_start_time_ns;
    if( ret){
        rt_printf( "选择从站时钟失败!");
    }
#endif //D_USE_DC_SYNC
    if( activeMaster() )
    {
        isMasterActived=true;
        // usleep(10000);
        rt_printf("Active master ........................OK!\n");
    }else{
        rt_printf("Active master ........................FAIL!\n");
        addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 16, -1 );
    }
#else
        pointProInterval =  50*1000 *1000/ cycle_ns ;
        pointProInterval_counter = 0;
#endif

}

static inline int tsgreater(struct timespec *a, struct timespec *b)
{
    return ((a->tv_sec > b->tv_sec) ||
        (a->tv_sec == b->tv_sec && a->tv_nsec > b->tv_nsec));
}
void CoeEcatDevice::cycleIntervalTime(void)
{

    if( !isMasterActived ){
        sleep_ns_real_time( 1000000 );
        return;
    }

    struct timespec  now;
    clock_gettime( CLOCK_MONOTONIC, &now);
    //防止超过周期时间带来的连续超时
    while (tsgreater(&now, &wakeupTime))
    {
#if 1
        int sleeptime = cycle_ns - dc_diff_ns;//主站比参考时钟数大( dc_diff_ns>0 )， 则要快点醒来，则计数会变小
        if( sleeptime<50000  )
            sleeptime=50000;//发送间隔过小,会获取不到数据
        wakeupTime.tv_nsec += sleeptime;
#else
        wakeupTime.tv_nsec += cycle_ns;
#endif
        while (wakeupTime.tv_nsec >= NSEC_PER_SEC)
        {
            wakeupTime.tv_nsec -= NSEC_PER_SEC;
            wakeupTime.tv_sec++;
        }
    }

    clock_nanosleep(CLOCK_MONOTONIC , TIMER_ABSTIME, &wakeupTime, NULL);
}

//(2016.11.21.xyf.已弃用)
void CoeEcatDevice::waitNodeReadyOk()
{
    rt_printf("Wait for write config......\n");
}

static char digits[19] = {
    '9','8','7','6','5','4','3','2','1','0',
    '1','2','3','4','5','6','7','8','9'
};

int convert( char* &buf, int value){
    static const char* zero = digits + 9;//zero->'0'
    int i = value;
    char *p = buf;
    do{
        int lsd= i%10;
        i/=10;
        *p++ = zero[lsd];
    }while( i!=0 );
    if(value <0)
        *p++ = '-';
    *p = '\0';

    std::reverse(buf,p);
    return p - buf;
}

void CoeEcatDevice::getNowTime(   )
{
#if 1
    //测试发现localtime 获取时间太长 500us
    //    time_t timep = time(NULL);
    //    struct tm* p=localtime(&timep);//从tm结构体中可以取到年月日时分秒等值。
    //    char szTmp[50] = {0};
    //    strftime(szTmp,50,"%Y-%m-%d, %H:%M:%S",p);    //这里输出的将是 p的时间值

    //    struct timeval tv_begin ;
    //    gettimeofday(&tv_begin, NULL);
    //  rt_printf("%s /n",szTmp);

    unsigned long nTime = t1.tv_sec;//得到当前系统时间
    S_DATE_TIME dtime;
    long timezone = 8;
    rtt_localtime( nTime,  &dtime, timezone  );


    //    QString strdata = QString("zxx当前时间: %1-%2-%3, %4:%5:%6")
    //            .arg( dtime.year ).arg( dtime.month ).arg( dtime.day )
    //            .arg( dtime.hour ).arg( dtime.minute ).arg( dtime.second );
    char *szTmp = new char[100];

    //    szTmp=(char *)(strdata.toStdString().data() );
    //     sprintf( szTmp, "zxx当前时间: %d-%d-%d, %d:%d:%d"
    //                       ,dtime.year ,dtime.month , dtime.day
    //                     ,dtime.hour , dtime.minute , dtime.second );

    char* __year=new char[ 10 ];
    char* __month=new char[ 10 ];
    char* __day=new char[ 10 ];
    char* __hour=new char[ 10 ];
    char* __minute=new char[ 10 ];
    char* __second=new char[ 10 ];

    convert(  __year, dtime.year );
    convert(  __month, dtime.month );
    convert(  __day, dtime.day );
    convert(  __hour, dtime.hour );
    convert(  __minute, dtime.minute );
    convert(  __second, dtime.second );

    strcat( szTmp, __year );
    strcat( szTmp, __month );
    strcat( szTmp, __day );
    strcat( szTmp, __hour );
    strcat( szTmp, __minute );
    strcat( szTmp, __second );

    rt_printf( "%s \n", szTmp );

    //  rt_printf( "zxx当前时间: %d- %d-%d  %d:%d:%d\n", 1990+p->tm_year, 1, p->tm_mon, p->tm_mday ,  p->tm_hour, p->tm_min, p->tm_sec) ;
#else
    const char* test  = QDateTime::currentDateTime().toString("yyyy-MM-dd HH:mm:ss").toStdString().data();
    rt_printf( "当前时间: %s\n",  test ) ;
#endif
}

void CoeEcatDevice::testTimer()
{
#if D_IF_DEBUG
    TimeTool::getInstance()->recordPeirodTime(timer_id[D_TIME_CYCLE]);
#endif
    if( checkTime )
    {
        clock_gettime(CLOCK_MONOTONIC , &t2);
        long timeout;
        if(t2.tv_nsec<t1.tv_nsec)
        {
            timeout=t2.tv_nsec+NSEC_PER_SEC-t1.tv_nsec;
        }else {
            timeout=t2.tv_nsec-t1.tv_nsec;
        }
        t1=t2;

#ifndef SIMULATION_MODE
        if( timeout < 100000 )
        {
            minCount++;
            rt_printf( "控制器  定时间隔小于100us！ %d  %d \n", timeout , minCount) ;
        }

        if( timeout < 20000  )
        {
            minCount++;
            if( minCount>15  )
            {
                //   getNowTime();
                rt_printf( "控制器运行发生错误，多次定时间隔小于20us！ %d  \n", timeout ) ;
                addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 32, -1 );

                if( minCount>20  )
                {
                    assert( 0 );
                }
            }
        }else{
            minCount=0;
        }
#endif

//        if( readdyOk ){
            counter++;
            if(counter>5000){//5s
                qDebug()<<"\n================realEncoderValue=="<<realEncoderValue;
                qDebug()<<"================encoderValue=="<<encoderValue;
                qDebug()<<"================updateHomeOffset=="<<updateHomeOffset;

//                wakeupTime.tv_nsec += cycle_ns*3;//
                counter=0;
//                qDebug()<<"\n\n\n\==========================seeeeeeeeeeeeeeeeeeeeeeeeerr";
            }
//        }
    }


    if(  !checkTime )
    {
        checkTime=true;
        clock_gettime(CLOCK_MONOTONIC ,&t1);
    }

}


/****************************************************************************/
quint64 rt_get_time_ns(void)
{
    struct timespec tv_begin ;
    clock_gettime(CLOCK_MONOTONIC , &tv_begin);
    return ( tv_begin.tv_sec*NSEC_PER_SEC+tv_begin.tv_nsec );
}

/** Get the time in ns for the current cpu, adjusted by system_time_base.
         *
         * \attention Rather than calling rt_get_time_ns() directly, all application
         * time calls should use this method instead.
         *
         * \ret The time in ns.
         *得到 当前CPU的纳秒时间，调整system_time_base。
        *
        * 注：所有的应用程序时间 应使用此方法代替， 而不是直接调用rt_get_time_ns()，
        *
        *   纳秒的 参考时间。
         */

uint64_t CoeEcatDevice::system_time_ns(void)
{
    quint64 time =rt_get_time_ns();// get_ns_to_2000y(); //

//    if (system_time_base > time) {
//        rt_printf("%s() error: system_time_base greater than"
//                  " system time (system_time_base: %lld, time: %llu\n",
//                  __func__, system_time_base, time);
//        return time;
//    } else {
        return time - system_time_base;
//    }
}


/** Convert system time to RTAI time in counts (via the system_time_base).
         * 将系统时间RTAI时间计数（通过system_time_base）。
         */
quint64 CoeEcatDevice::system2count(   uint64_t time  )
{
    quint64 ret;

    if ((system_time_base < 0) &&  ((uint64_t) (-system_time_base) > time))
    {
        rt_printf("%s() error: system_time_base less than"
                  " system time (system_time_base: %lld, time: %llu\n",
                  __func__, system_time_base, time);
        ret = time;
    }
    else {
        ret = time + system_time_base;
    }

    return  ret;
}


/** Return the sign of a number
 *
 * ie -1 for -ve value, 0 for 0, +1 for +ve value
 *
 * \retval the sign of the value  返回 val 的符号
 */
#define sign(val) \
    ({ typeof (val) _val = (val); \
    ((_val > 0) - (_val < 0)); })

/** Synchronise the distributed clocks 分布式时钟同步  */
void CoeEcatDevice::sync_distributed_clocks(void)
{

#if SYNC_MASTER_TO_REF
    uint32_t ref_time = 0;//参考时钟时间
    uint32_t prev_app_time_32 = dc_time_ns; //记录上一个 app 时间
#endif

    //获取系统时间写入主站
    dc_time_ns = system_time_ns();//获取一个新 app 时间
    // set master time in nano-seconds
    ecrt_master_application_time( master, dc_time_ns+2000000 );//设置 app 时间

#if SYNC_MASTER_TO_REF

#if 1

    static qint64  ref_time_r  = 0;
    // get reference clock time to synchronize master cycle 获取参考时钟 来同步主站时钟??
    if( sync_ref_counter  ){
        sync_ref_counter--;
    }else{
        sync_ref_counter=1;
        int xxxx=0;
        static  int once=10;
        if( once>0 ){//前10个周期用于主站同步到参考时钟
            --once;
            //  dc_diff_ns = (uint32_t) prev_app_time - ref_time;//得到上一个时钟漂移（因为返回的时间是上次发送时测量的?）
            ecrt_master_reference_clock_time(master, &ref_time);//获取 32位的参考时钟系统时间

           qint32 ref_v = ref_time-ref_time_r;
           ref_time_r= ref_time;

          //  dc_diff_ns=0;
            //第一次消抖,用于获取准确的参考时间
            xxxx = prev_app_time_32 - ref_time;//得到主站时钟与参考时钟的偏差 //得到上一个时钟漂移（因为返回的时间是上次发送时测量的?）

            system_time_base += xxxx;//2147483648 , 4294967295

         //   rt_printf(" xxxx时钟 :偏移=%d , 参考=%ud  ,上次=%ud  , %uld , %d \n",  xxxx, ref_time,  prev_app_time_32 , system_time_base, ref_v  );//2948 956 ns

            //return ;
        }
        /*else{
        //  dc_diff_ns = (uint32_t) prev_app_time - ref_time;//得到上一个时钟漂移（因为返回的时间是上次发送时测量的?）
        ecrt_master_reference_clock_time(master, &ref_time);//获取 32位的参考时钟系统时间

       qint32 ref_v = ref_time-ref_time_r;
       ref_time_r= ref_time;

            //消抖动，用于改变睡眠量
            xxxx = prev_app_time_32 - ref_time;//得到主站时钟与参考时钟的偏差 //得到上一个时钟漂移（因为返回的时间是上次发送时测量的?）

            //system_time_base += xxxx;


        //system_time_base += xxxx;

        static int diff_count=0;
        static int diff_count_2=0;
        static int diff_count_3=0;
        static qint64 diff_av=0;
        static qint64 diff_av_2=0;
        static qint64 diff_av_3=0;

        if( xxxx>0 ){
            diff_av+=xxxx;
            diff_count++;
        }else{
            diff_av_2+=xxxx;
            diff_count_2++;
        }
        diff_av_3 += xxxx;
        diff_count_3++;
        rt_printf(" 时钟 :偏移=%d , 参考=%ud  ,上次=%ud  ,%d,  %d,  %uld , %d\n",  xxxx, ref_time,  prev_app_time_32 ,diff_count, diff_count_2 ,system_time_base, ref_v );//2948 956 ns

        dc_diff_ns = xxxx;
           if( diff_count_3%200==0 )
        {

              // dc_diff_ns= diff_av_3/200;

            int64_t df0= diff_av/diff_count;
            rt_printf(" 时钟x :偏移=%d , 参考=%ud  ,上次=%ud  ,  平均=%d \n",  dc_diff_ns, ref_time,  prev_app_time_32 , df0 );//2948 956 ns
            diff_count_3=0;
            diff_av_3=0;

            if( (diff_count_2>0 &&diff_count>0 ) )
            {
                int64_t df1= diff_av/diff_count;
                rt_printf(" 时钟 1 :%ud , %ud  ,%ud  , %d \n",  xxxx, ref_time,  prev_app_time_32 , df1 );//2948 956 ns
                //    diff_av=0;
                //   diff_count=0;

                int64_t df2= diff_av_2/diff_count_2;
                rt_printf(" 时钟 2 :%ud , %ud  ,%ud  , %d \n",  xxxx, ref_time,  prev_app_time_32 , df2 );//2948 956 ns
                //    diff_av=0;
                //   diff_count=0;

            }

       //     assert( 0 );
        }
              }*/
    }

 //   dc_diff_ns=0;
      //        qDebug()<< "主站时钟漂移"<<dc_diff_ns <<ref_time<< app_time_32 <<prev_app_time;
#endif
//      sleep(1);
//      sleep_ns_real_time(200000);//usleep()定时不准
    //system_time_base=dc_diff_ns;


#else
    // sync reference clock to master
    ecrt_master_sync_reference_clock(master); //将主站时钟覆盖参考时钟
#endif
    // call to sync slaves to ref slave
    ecrt_master_sync_slave_clocks(master);//同步从站时钟，发送前调用


}

//系统时钟＝参考时钟
//系统时钟副本 = 本地记录的系统时间
//系统时钟偏差＝本地时钟与系统时钟差
//时钟漂移补偿= 周期性检查　本地时钟与系统时钟副本的偏差
/** Update the master time based on ref slaves time diff.  将主站时钟更新到从站参考时钟
* called after the ethercat frame is sent to avoid time jitter in .发送帧数据后立刻调用避免时间抖动
*/
void CoeEcatDevice::update_master_clock(void)
{
#if SYNC_MASTER_TO_REF
    // calc drift (via un-normalised time diff) 计算漂移  // 主站与参考时钟的的偏差
    int32_t delta = dc_diff_ns - prev_dc_diff_ns;//抖动=上一次-这一次
    prev_dc_diff_ns = dc_diff_ns;//记录上次偏差

    // normalise the time diff  恢复/正常化 时间差异
    dc_diff_ns =  ((dc_diff_ns + (cycle_ns / 2)) % cycle_ns) - (cycle_ns / 2);

    // only update if primary master 只更新原始的主
    if (dc_started)
    {
        // add to totals  总计
        dc_diff_total_ns += dc_diff_ns;
        dc_delta_total_ns += delta;
        dc_filter_idx++;

        if (dc_filter_idx >= DC_FILTER_CNT)
        {
            // add rounded delta average 计算 delta 平均值
            dc_adjust_ns +=  ((dc_delta_total_ns + (DC_FILTER_CNT / 2)) / DC_FILTER_CNT);

            // and add adjustment for general diff (to pull in drift)
            dc_adjust_ns += sign(dc_diff_total_ns / DC_FILTER_CNT);

            // limit crazy numbers (0.1% of std cycle time)
            if (dc_adjust_ns < -1000) {
                dc_adjust_ns = -1000;
            }
            if (dc_adjust_ns > 1000) {
                dc_adjust_ns =  1000;
            }

            // reset
            dc_diff_total_ns = 0LL;
            dc_delta_total_ns = 0LL;
            dc_filter_idx = 0;
        }

        // add cycles adjustment to time base (including a spot adjustment) 添加周期调整到时基（包括现场调整）
        system_time_base += dc_adjust_ns + sign(dc_diff_ns);
    } else {
        dc_started = (dc_diff_ns != 0);

        if (dc_started) {
            // output first diff
            rt_printf("First master diff: %d.\n", dc_diff_ns);

            // record the time of this initial cycle
            dc_start_time_ns = dc_time_ns;
        }
    }
#endif
}

void CoeEcatDevice::vrep_update(){

#ifdef SIMULATION_MODE
    if(  tcpVrep==NULL )
    {
        tcpVrep=new TcpToVrep;
    }

    if(tcpCounter<1000 )
    {
        tcpCounter++;
    }else{
        tcpCounter=0;
        if( !tcpVrep ->isOpen()  )
        {
            tcpVrep->connectToVrep();
            QCoreApplication::processEvents();
            if( tcpVrep->waitForConnected( 2 ) )
            {
                qDebug()<< "Tcp vrep 连接远程主机  ok !"<<   tcpVrep->peerAddress().toString() ;
                // rt_printf(  "Tcp vrep 连接远程主机  ok , %s !",  tcpVrep->peerAddress().toString().toStdString().data() );
            }else{
                tcpVrep->close();
                QCoreApplication::processEvents();
                qDebug()<< "Tcp vrep 连接远程主机  failed !"<<  tcpVrep->remoteIpAddress;
                // rt_printf( "Tcp vrep 连接远程主机  failed , %s  !" , tcpVrep->peerAddress().toString().toStdString().data() );
            }
            return;
        }else{
            if( tcpVrep->remoteIpAddress!= tcpVrep->peerAddress().toString() )
            {
                tcpVrep->close();
                QCoreApplication::processEvents();
                qDebug()<< "ip修改， Tcp vrep 断开连接  !"<<tcpVrep->peerAddress().toString()<< tcpVrep->remoteIpAddress;
                return;
            }
        }
    }

    if( pointProInterval_counter%50)
    {
        // cycle_ns=50* 1000 *1000; //vrep ,80ms
        QCoreApplication::processEvents();
    }

    if( p_powerManger!=NULL && tcpVrep->state()==QTcpSocket::ConnectedState )
    {
        RobotEngine* rob = p_powerManger->robotManager->robotsMap.value( 1, NULL );
        if(rob!=NULL)
        {
            for(int i=0;i<ctlWord.size(); i++)
            {
                if( ctlWord[i] & 0x0F  == 0x0F)
                {
                    staWord[i] = 4663 ;
                }else  {
                    staWord[i] = 561 ;

                }
                if( ioDeviceData.size() )
                {
                    for(int kk=0;kk<ioDeviceData.size(); kk++ ){
                        if( ioDeviceData[kk].nodeId == 87 ){
                            if(   ioDeviceData[kk].ioDi.size()>9){
                                ioDeviceData[kk].ioDi[8]=true;
                                //   ioDeviceData[kk].ioDi[9]=true;
                                //   ioDeviceData[kk].ioDi[10]=true;
                            }
                        }
                    }
                }
            }

            if( rob->threadMotionEngine && rob->threadMotionEngine->mcControllerInterpreter )
            {

              //  timeTest_2  ->recordInTime();
                //   setServoDataRobot(  -1 );// 写伺服位置
                QVector<MotorData> mdata= rob->threadMotionEngine->mcControllerInterpreter->getMotorCommand();
                pthread_mutex_lock(&ecatThreadMutex);
                if( mdata.size()  )
                {
                    get_data_false_count=1;
                    v_encoder_t.resize( mdata.size() );
                    for(int  i=0;i< mdata.size(); i++)
                    {
                        v_encoder_t[i]=mdata[i].position  + updateHomeOffset[i];
                    }
                    //      qDebug()<<"43et 写伺服位置"<< v_encoder_t;
                }else{
                    if( get_data_false_count ){
                        get_data_false_count=0;
                        rt_printf("ethercat 读点 为空 ！\n");
                    }
                }

                pthread_mutex_unlock(&ecatThreadMutex);

              //  timeTest_2  ->recordOutTime();
#if   D_WRITE_SERVO_DATA_TO_FILE
                //范例
                if( mdata.size() >0 )
                {
                    if ( fpServoDataFile !=NULL )
                    {
                        fprintf( fpServoDataFile, "%d" , ++servo_data_id );
                        for(int  i=0;i< mdata.size(); i++)
                        {
                            //  int tag=mdata[i].position  + updateHomeOffset[i];
                            int pos=mdata[i].position ;
                            fprintf(  fpServoDataFile, "%d , ", pos );
                        }
                        fprintf( fpServoDataFile, "\n"  );
                        fpFlush=true;
                        fflush( fpServoDataFile );
                    }
                }else{
                    if ( fpServoDataFile !=NULL ){
                        if(fpFlush){
                            fflush( fpServoDataFile );
                            fpFlush=false;
                        }
                    }
                }
#endif

                if( pointProInterval_counter < pointProInterval )
                {
                    pointProInterval_counter++;
                    return;
                }else{
                    pointProInterval_counter=0;

                    if( mdata.size()<1  )
                    {//当没数据可发时，发送最后一个数据
                        if( v_encoder_t.size() )
                        {
                            //                       qDebug()<< "====4346==xxxx==发送最后一个位置："<< v_encoder_t;
                            QVector<float> v_motorData = rob->threadMotionEngine->mcControllerInterpreter->getVrepFromMotor( v_encoder_t );
                            if(  v_motorData.size() )
                            {
                                tcpVrep->move_axies(v_motorData);
                                QCoreApplication::processEvents();
                            }
                            v_encoder_t.clear();
                        }
                    }
                }

               // timeTest_3  ->recordInTime();
                //r
                QCoreApplication::processEvents();
                QVector<float> raData = tcpVrep->get_axies_data();
                if( raData.size() >0)
                {
                    QVector<int> v_encoder=  rob->threadMotionEngine->mcControllerInterpreter->getVrepRadianToEncoder(  raData );
                    //    qDebug()<< "======xxxx==s获取位置："<< raData<< v_encoder;
                    if( v_encoder.size()  )
                    {
                        for(int i=0; i<v_encoder.size(); i++ )
                        {
                            realEncoderValue[i]=v_encoder.at(i);
                            encoderValue[i]=v_encoder.at(i) - updateHomeOffset.at(i);

                        }
                    }
                    getServoStateDataRobot(  -1 ); //读数据到组件 servoStateMap 结构体

                }
                if( mdata.size()  )
                {
                    for(int  i=0;i< mdata.size(); i++)
                    {
                        targetPos[i]=mdata.at(i).position  + updateHomeOffset.at(i);
                    }
                    //   qDebug()<< "======xxxx==发送位置："<< targetPos;
                    QVector<float> v_motorData = rob->threadMotionEngine->mcControllerInterpreter->getVrepFromMotor( targetPos );
                    if(  v_motorData.size() )
                    {
                        tcpVrep->move_axies(v_motorData);
                        QCoreApplication::processEvents();
                    }
                }

               // timeTest_3  ->recordOutTime();
            }

        }
    }
#endif
}

 bool CoeEcatDevice::reInit(){
     rt_printf( "reinit...\n" );
     sleep_ns_real_time( NSEC_PER_SEC );//1s
     if( master ){
         if(isMasterActived){
             rt_printf( "ecrt_release_master...\n" );
             ecrt_release_master(master);
             sleep_ns_real_time( NSEC_PER_SEC );//1s
             init(cycle_ns/1000);
             cycleStartTime();
             return isInitOk;
         }
     }
     return false;
 }

//经测试：ＰＤＯ每一帧都会自动改为ＯＰ状态
//  quint16 val = 0x02;
//  regRealtimeWrite( pAliasSetRegRequest[i],  i,  (void*)&val ,   2,   0x0120 );

//   ec_slave_info_t slave_info;
//   int ret= ecrt_master_get_slave(  master,    i,   &slave_info  );
//  ec_slave_config_load_default_sync_config() ;
void CoeEcatDevice::igh_update( )
{
    if( !isMasterActived )
        return;

#if D_IF_DEBUG
    TimeTool::getInstance()->recordInTime(timer_id[D_TIME_READ]);
#endif

    ecrt_master_receive(master);
    ecrt_domain_process(domain);
    //sleep_ns_real_time(200000);//usleep()定时不准

    rt_check_domain_state();//检查域状态
    cycle_counter++;
    if( cycle_counter > 1000 )
    {
        cycle_counter=0;
        if( rt_check_master_state() ){//检查主站状态
            readdyOk=false;
        }

        if(!readdyOk)
        {
            if( 0 == rt_check_slaves_state() ){//检查从站状态
                readdyOk = true;

                //添加homeosffset缓存,未初始化时，也可以用
                pthread_mutex_lock( &ecatThreadMutex );
                if( tmp_updateHomeOffset.size()>0 ){
                    QList<int > keys= tmp_updateHomeOffset.keys();
                    for(int i=0;i<keys.size();i++ ){
                        int ky=keys.at(i);
                        rt_printf(  "机器人: %d，零点标定ok. \n" , ky);
                        funSetRobotValue( ky, updateHomeOffset, tmp_updateHomeOffset[ky] );
                        funSetRobotValue( ky, targetPos, 0 );//重新赋值，防止线程不同步造成的误动
                        funSetRobotValue( ky, encoderValue, 0 );//重新赋值
                    }
                    tmp_updateHomeOffset.clear();
                }
                pthread_mutex_unlock( &ecatThreadMutex );

                resetErrorRobot(-1);
//               if( faultOccurred )
//                {
//                    qDebug()<< "2522 readdyOk 重置状态 =" <<readdyOk << isInitOk<<encoderValue;
//                    for( int i = 0; i < ctlWord.size(); i++ )
//                    {
//                        ctlWord[i] |= 0x80;
//                    }
//                }
            }
        }
    }

#if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id[D_TIME_READ]);
    TimeTool::getInstance()->recordInTime(timer_id[D_TIME_MATH]);
#endif
    setServoDataRobot(  -1 );// 用算法写伺服位置
#if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id[D_TIME_MATH]);
    TimeTool::getInstance()->recordInTime(timer_id[D_TIME_RW]);
#endif
    updateDomainData(); //读写过程数据 (帧数据交互)

    {//延时检测错误
        if( check_error_delay<1)
            errUpdate( -1 );//检查报警
        else
            check_error_delay--;
    }
    getServoStateDataRobot( -1 ); //更新数据到 servoStateMap 结构体 (真实数据写到缓存数据)
#if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id[D_TIME_RW]);
    TimeTool::getInstance()->recordInTime(timer_id[D_TIME_SEND]);
#endif
    ecrt_domain_queue(domain);
#if D_USE_DC_SYNC
#if 0
    sync_distributed_clocks();//将主站同步到从站0
#else
    //获取主站时间，覆盖参考时钟(从站0)，并同步到所有从站
   ecrt_master_application_time(master, EC_TIMEVAL2TCNANO( wakeupTime )+2000000 );//2000000
    if (sync_ref_counter) {
        sync_ref_counter--;
    } else {
        sync_ref_counter = 1;                           // 每个周期一个同步信号
        ecrt_master_sync_reference_clock(master);       //主站同步参考时钟
    }
    ecrt_master_sync_slave_clocks(master);          //从站同步参考时钟
#endif
#endif
    ecrt_master_send(master);
  //  update_master_clock();//调整主站时钟
#if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id[D_TIME_SEND]);
#endif

}


void CoeEcatDevice::ecatUpdate( )
{
#if D_IF_DEBUG
    TimeTool::getInstance()->recordInTime(timer_id[D_TIME_SLEEP]);
#endif

    cycleIntervalTime();///睡眠至 唤醒时间，并设定下次唤醒时间
#if D_IF_DEBUG
    TimeTool::getInstance()->recordPeirodTime(timer_id[D_TIME_TIMER]);
#endif

#if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id[D_TIME_SLEEP]);
    TimeTool::getInstance()->recordInTime(timer_id[D_TIME_WORK]);
#endif

#ifdef SIMULATION_MODE
    vrep_update();
#else
    igh_update();
#endif  //end  SIMULATION_MODE 2016.12.31.xyf 添加3ｄ模拟支持

#if D_IF_DEBUG
    TimeTool::getInstance()->recordOutTime(timer_id[D_TIME_WORK]);
#endif

    testTimer();//周期时间计算

}


//更新所有机器人是否有报警
void CoeEcatDevice::errUpdate( int robID )
{
    if(robID<0)
    {
        RobotInfo rob;
        foreach (rob, robots)
        {
            warningState = false;
            faultOccurred = false;
            for(int i=0; i<rob.motorAlias.size(); i++ )
            {
                errUpdateNode( aliasMapNode.value(rob.motorAlias.at(i)) );
            }
            if(faultOccurred ||  customErrorSearch() )
            {
                // 错误发生
                faultOccurred= true;
                quickStopRobot( rob.robotId );
            }
        }
    }else{
        warningState = false;
        faultOccurred = false;
        for(int i=0; i<robots.value(robID).motorAlias.size(); i++  )
        {
            errUpdateNode( aliasMapNode.value(robots.value(robID).motorAlias.at(i)) );
        }
        if(faultOccurred ||  customErrorSearch() )
        {
            // 错误发生
            faultOccurred= true;
            quickStopRobot( robID );
        }
    }
}


//更新指定轴的报警
void CoeEcatDevice::errUpdateNode( int node )
{
    S_EcatDevInfo* robDev=robotDevs.value(nodeMapAlias.value(node) ,NULL);
    if(robDev==NULL){
        rt_printf("=======dev null　－　%d\n", node);
        return ;
    }
    switch( robDev->devInfo.devType  )
    {
    case E_EcatDev_Servo:
        //pdo
        if( staWord[ node ] & 0x08  )
        {//error
            if(!faultOccurred ){
                faultOccurred= true;
                addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 34,
                       nodeToRobotIdAndAxit[node].robotId,
                       nodeToRobotIdAndAxit[node].axis,
                       errorCode[ node ],
                       ecatDevs[ node ].devInfo.vendorId );
            }
        }
        else if(   staWord[ node ] & 0x80  )
        {//warm
            if(!warningState )
            {
                warningState = true;
                addMsg(ENUM_MSG_WARNING, "CoeEcatDevice", "CoeEthercat", 41,
                       nodeToRobotIdAndAxit[node].robotId,
                       nodeToRobotIdAndAxit[node].axis,
                       errorCode[ node ],
                       ecatDevs[ node ].devInfo.vendorId);
            }
        }else{
            ///LS出现只有报警码，没有报警状态的情况
        }

        break;//  motor
    case E_EcatDev_Coupler:
        break;//
    case E_EcatDev_IoModule:
        break;
    case E_EcatDev_TypeErr:
    default:
        break;
    }

}


void CoeEcatDevice::quickStopRobot(int robID)
{
    if( !isInitOk ){
        return;
    }
    // servoStatus = EM_DRIVER_UNSTANDBY;
    pthread_mutex_lock(&ecatThreadMutex);
    if( robID<0  )
    {
        RobotInfo rob;
        foreach (rob, robots )
        {
            QVector<int> motorAlias = rob.motorAlias;
            for(int i=0; i<motorAlias.size(); i++  )
            {
                int nd = aliasMapNode.value( motorAlias.at(i) );
                servoQuickStop(nd);
            }
        }
    }else{
        QVector<int> motorAlias = robots.value(robID).motorAlias;
        for(int i=0; i<motorAlias.size(); i++  )
        {
            int nd = aliasMapNode.value( motorAlias.at(i) );
            servoQuickStop(nd);
        }
    }
    pthread_mutex_unlock(&ecatThreadMutex);
}


void CoeEcatDevice::servoOnRobot( int robID )
{
    if( state()!=EC_OP ){
       // rt_printf( "ethercat error:. %d,%d,%d,%d, %d\n " ,isInitOk, readdyOk, error_flag , isRequireReboot, state() );
        qDebug()<<"ethercat not ready! exit -3"<<isInitOk<<readdyOk<<error_flag<<isRequireReboot<<state();
        return;
    }

    pthread_mutex_lock(&ecatThreadMutex);
    //  on前先复位编码器
    funReSetRobotValue(  robID,   targetPos  ,  encoderValue );
    funSetRobotValue(  robID,   ctlWord  ,  0x0F );
    robotServoOnFlag[ robID ]=true;

              //  rt_printf( "ethercat error:. %d,%d,%d,%d, %d\n " ,isInitOk, readdyOk, error_flag , isRequireReboot, state() );
                qDebug()<<"36hj ethercat "<<isInitOk<<readdyOk<<error_flag<<isRequireReboot<<state();

    pthread_mutex_unlock(&ecatThreadMutex);
}


void CoeEcatDevice::servoOffRobot( int robID )
{
    if( !isInitOk ){
        return;
    }
    pthread_mutex_lock(&ecatThreadMutex);
    funSetRobotValue(  robID,   ctlWord  , 0x06 );
    robotServoOnFlag[ robID ]=false;
    pthread_mutex_unlock(&ecatThreadMutex);
}

//
void CoeEcatDevice::servoOnNode( int node  )
{
    if( state()!=EC_OP ){
        qDebug()<<"ethercat没准备好!! exit -3"<<isInitOk<<readdyOk<<state();
        return;
    }
    targetPos[node]=encoderValue[node];
    ctlWord[node]=0x0F;
    robotServoOnFlag[ nodeToRobotIdAndAxit[node].robotId ] = true;
}
void CoeEcatDevice::servoOffNode(int node  )
{
    robotServoOnFlag[ nodeToRobotIdAndAxit[node].robotId ] = false;
    ctlWord[node]=0x06;//0x07不能直接on
}
void CoeEcatDevice::servoHalt(int node )
{
    ctlWord[node]|=0x80;
}
void CoeEcatDevice::servoReRun(int node  )
{
    ctlWord[node]&=0x7F ;
}
void CoeEcatDevice::servoQuickStop(int node  )
{
    //   ctlWord[node]=(ctlWord[node]&0x79)|0x02;  // 0x0b
    ctlWord[node]=0x06;
    robotServoOnFlag[ nodeToRobotIdAndAxit[node].robotId ] = false;
}
void CoeEcatDevice::servoSwitch(int robotId  , bool flag)
{
    if( state()!=EC_OP ){
         qDebug()<<"ethercat not ready!!! exit -3"<<isInitOk<<readdyOk<<state();
         return;
     }

    ///锁定
    if(  robotServoOnFlag[ robotId ] )
    {
        return;
    }

    int val=0x06;
    if( flag ){
        val=0x0f;
    }

    pthread_mutex_lock(&ecatThreadMutex);
    //  on前先复位编码器
    funReSetRobotValue(  robotId,   targetPos  ,  encoderValue );
    funSetRobotValue(  robotId,   ctlWord  ,  val );
    pthread_mutex_unlock(&ecatThreadMutex);
}
//
bool CoeEcatDevice::isServoOnNode(int nd  )
{
    return (staWord[nd]&0x67)==0x27?true:false;
}

bool CoeEcatDevice::isServoOnRobot( int robotId )//伺服使能
{
    if( !isInitOk ){
        return false;
    }
    QVector<int> motorAlias = robots.value(robotId).motorAlias;
    for(int i=0; i<motorAlias.size(); i++ )
    {
        int nd=aliasMapNode.value(motorAlias.at(i));
        if(  !isServoOnNode( nd ) )
        {
            return false;
        }
    }
    return true;
}

bool CoeEcatDevice::isServoQuickStop( int nd  )
{
    return (staWord[nd]&0x67)==0x07?true:false;
}

bool CoeEcatDevice::isServoPowerOn(int nd  )
{
    return (staWord[nd]&0x10)==0x10?true:false;
}

bool CoeEcatDevice::isServoTargetReached( int nd )
{
    return (staWord[nd]&0x400)==0x400?true:false;
}

bool CoeEcatDevice::isServoFllowingError( int nd )
{
    return (staWord[nd]&0x2000)==0x2000?true:false;
}

bool CoeEcatDevice::isServoHoming(int nd  )
{
    return (staWord[nd]&0x2400)==0?true:false;
}

bool CoeEcatDevice::isServoHomingOk( int nd )
{
    return (staWord[nd]&0x1400)==0x1400?true:false;
}

bool CoeEcatDevice::isServoHomingErr( int nd )
{
    return (staWord[nd]&0x2000)==0x2000?true:false;
}

void CoeEcatDevice::setServoDataRobot(int robID )
{
    if( robID<0 )
    {
        RobotInfo rob;
        foreach (rob, robots )
        {
            setServoData( rob.robotId  );
        }
    }else{
        setServoData( robID  );
    }
}

//尝试加锁，失败后不更新数据？ trylock()会导致过程误差过大
void CoeEcatDevice::getServoStateDataRobot(int robID )
{
    // pthread_mutex_lock(&ecatThreadMutex) ;
    if( 0 == pthread_mutex_trylock(&ecatThreadMutex) )
    {
        if( robID<0 )
        {
            RobotInfo rob;
            foreach (rob, robots )
            {
                getServoStateData( rob.robotId  );
            }
        }else{
            getServoStateData( robID  );
        }
        pthread_mutex_unlock(&ecatThreadMutex);
    }
}

void CoeEcatDevice::getServoStateData(int robID)
{
    QVector<ServoState*> data = servoStateMap[robID];
    QVector<int> motorAlias = robots.value(robID).motorAlias;
    for(int i=0; i<motorAlias.size(); i++ )
    {
        int nd=aliasMapNode.value(motorAlias.at(i));
        if(data[i]==NULL )
        {
            qDebug()<< " err:  data[i]==NULL, "<< i<< nd;
            continue;
        }

        data[i]->isServoOn=isServoOnNode(nd);
        data[i]->isServoQuickStop=isServoQuickStop(nd);
        data[i]->isServoPowerOn=isServoPowerOn(nd);
        data[i]->isServoTargetReached=isServoTargetReached(nd);
        data[i]->isServoFllowingError=isServoFllowingError(nd);
        data[i]->isServoHoming=isServoHoming(nd);
        data[i]->isServoHomingOk=isServoHomingOk(nd);
        data[i]->isServoHomingErr=isServoHomingErr(nd);

        data[i]->position= encoderValue[nd];//getMotorPositionNode(nd);
        data[i]->speed= velocityActValue[nd];
        data[i]->torque= torqueActValue[nd];
        data[i]->power= 50;
        data[i]->fllowingOffset = followingErrorActualValue[nd]; //实际跟随误差(计算值)

        if( faultOccurred )
        {//error
            data[i]->errorCode= errorCode[nd];
            data[i]->errorString="errorString";
        }
        else if(  warningState  )
        {//warm
            data[i]->warnCode= errorCode[nd];
            data[i]->warnString="warnString";
        }
    }
}

void CoeEcatDevice::setServoData(int robID)
{

#if 1
    if( p_powerManger!=NULL )
    {
        RobotEngine* rob = p_powerManger->robotManager->robotsMap.value( robID, NULL );
        if(rob!=NULL)
        {
            if(  rob->threadMotionEngine->mcControllerInterpreter )
            {
                if(  rob->threadMotionEngine->mcControllerInterpreter )
                {
                    QVector<MotorData> mdata ;
                    mdata = rob->threadMotionEngine->mcControllerInterpreter->getMotorCommand();
                    QVector<int> motorAlias = robots.value(robID).motorAlias;
#if   D_WRITE_SERVO_DATA_TO_FILE
                    //范例
                    if( mdata.size() >0 )
                    {
                        if ( fpServoDataFile !=NULL )
                        {
                            fprintf( fpServoDataFile, "%d" , ++servo_data_id );
                            for(int  i=0;i< mdata.size(); i++)
                            {
                                //  int tag=mdata[i].position  + updateHomeOffset[i];
                                int pos=mdata[i].position ;
                                fprintf(  fpServoDataFile, "%d ,", pos );
                            }
                            fprintf( fpServoDataFile, "\n"  );
                        }
                    }
#endif
                    if(mdata.size() && motorAlias.size()==mdata.size() )
                    {

                        get_data_false_count=1;
                        //  rt_printf(" \n==发送点位数据==\n"  );
                        for(int  i=0;i< mdata.size(); i++)
                        {
                            //     rt_printf("%d , ", mdata[i].position );
                            int nd=aliasMapNode.value(motorAlias.at(i));
                            targetPos[nd]=mdata[i].position;
                        }
                    } else{
                        //      rt_printf("1423 不动=========\n");
                        if( get_data_false_count ){
                            get_data_false_count=0;
//                            rt_printf("ethercat 读点 为空 ！\n");
                        }
                    }
                }
            }
        }
    }
#else
    QQueue< QVector<MotorData> >* pData = motorDataMap.value( robID, NULL );
    if(pData )
    {
        // motorDataMutexMap.value( robID )->lock() ;
        //写 MotorData
        if(  !pData->isEmpty())
        {
            QVector<MotorData> tmpData = pData->dequeue();
            QVector<int> motorAlias = robots.value(robID).motorAlias;

            for(int i=0; i<motorAlias.size(); i++ )
            {
                int nd = aliasMapNode.value( motorAlias.at(i) );

                // targetTorque[nd] = tmpData.at(i).torque;
                // targetSpeed[nd] = tmpData.at(i).speed;
                targetPos[nd] = tmpData.at(i).position;
            }
        }
        //  motorDataMutexMap.value( robID )->unlock();
    }
#endif
}

void CoeEcatDevice::setControlWordRobot(int robID,  int value )
{
    if( !isInitOk ){
        return ;
    }
    pthread_mutex_lock(&ecatThreadMutex);
    if( robID<0 )
    {
        RobotInfo rob;
        foreach (rob, robots)
        {
            QVector<int> motorAlias= rob.motorAlias;
            for(int i=0; i<motorAlias.size(); i++  )
            {
                int nd=aliasMapNode.value(motorAlias.at(i));
                servoStateControlNode(nd, value);
            }
        }
    }else{
        QVector<int> motorAlias= robots.value(robID).motorAlias;
        for(int i=0; i<motorAlias.size(); i++  )
        {
            int nd=aliasMapNode.value(motorAlias.at(i));
            servoStateControlNode(nd, value);
        }
    }

    pthread_mutex_unlock(&ecatThreadMutex);
}

void CoeEcatDevice::servoStateControlNode(int node, int ctlCmd)
{
    switch( ctlCmd )
    {
    case  SHUTDOWN_CTLCMD:
    {
        ctlWord[ node ]  = 0x06;
    }
        break;
    case SWITCHON_CTLCMD:
    {
        ctlWord[ node ] |= 0x07;
        ctlWord[ node ] &= 0x77;
    }
        break;
    case ENABLEOPERATION_CTLCMD:
    {
        ctlWord[ node ] |= 0x0F;
        ctlWord[ node ] &= 0x7F;
    }
        break;
    case DISABLE_VOLTAGE_CTLCMD:
    {
        ctlWord[ node ] &= 0x7d;
    }
        break;
    case QUICK_STOP_CTLCMD:
    {
        ctlWord[ node ] |= 0x02;
        ctlWord[ node ] &= 0x79;
    }
        break;
    case DISABLE_OPERATION_CTLCMD:
    {
        ctlWord[ node ] |= 0x7;
        ctlWord[ node ] &= 0x77;
    }
        break;
    case FAULT_RESET_CTLCMD:
    {
        ctlWord[ node ] |= 0x80;
    }
        break;
    default:
        break;
    }
}

///获取轴的各种数据(机器人所有轴)
QVector<ServoState> CoeEcatDevice::getServoDataRobot( int robID  )
{
    QVector<ServoState> stateData;
    if( !isInitOk ){
        // stateData.resize(4);
        return stateData;
    }
    pthread_mutex_lock(&ecatThreadMutex);
    //    if( 0==pthread_mutex_trylock(&ecatThreadMutex) )
    {
        if( robID<0  )
        {
            rt_printf("get---ServoDataRobot: error: robID can not be -1 !\n");
        }else{
            QVector<ServoState*>data  =  servoStateMap.value( robID );
            stateData.resize( data.size() );
            for(int i=0; i<data.size(); i++  )
            {
                if( data[i] )
                    stateData[i] = *(data[i]);
            }
        }
        pthread_mutex_unlock(&ecatThreadMutex) ;
    }
    return stateData;
}


void CoeEcatDevice::setAmpModeRobot(int robID, E_ServeoMode mode)
{
    pthread_mutex_lock(&ecatThreadMutex);
    funSetRobotValue(  robID, ampMode  , mode );
    pthread_mutex_unlock(&ecatThreadMutex) ;
}


QVector<int> CoeEcatDevice::getMotorPositionRobot( int robID )
{
    return funGetRobotValue(  robID,   encoderValue  );
}

QVector<int> CoeEcatDevice::getMotorPositionRobot_real( int robID )
{
    return funGetRobotValue(  robID,   realEncoderValue  );
}

void CoeEcatDevice::setMotorPositionRobot(int robID,QVector<int> vPos)
{
    if( !isInitOk )
    {
        return ;
    }
    funSetRobotValue(robID, targetPos, vPos );
}

void CoeEcatDevice::resetMotorPositionRobot(int robID)
{
    if( !isInitOk )
    {
        return ;
    }
    funReSetRobotValue(robID, targetPos, encoderValue );
}

int CoeEcatDevice::goHomeNode( int nd )
{
    return 1;
}

int CoeEcatDevice::goHomeRobot( int robID )
{
    if(  state()!=EC_OP ){
        return 0;
    }
    //    if( robID<0 )
    //    {
    //        qDebug()<<"goHomeRobot: robot ID can not be -1 !";
    //        return 0;
    //    }else{
    //        QVector<int> motorAlias= robots.value(robID).motorAlias;
    //        for(int i=0; i<motorAlias.size(); i++  )
    //        {
    //            int nd=aliasMapNode.value(motorAlias.at(i));
    //            if(  goHomeNode(nd) == 0  )
    //            {
    //                qDebug()<<"Error: 归零失败!"<<nd;
    //                return 0;
    //            }
    //        }
    //    }
    return 1;
}


int CoeEcatDevice::setHomeOffsetRobot( int robID, QVector<int>&valueList )
{
    if( state()!=EC_OP ){
        pthread_mutex_lock( &ecatThreadMutex );
        tmp_updateHomeOffset[robID]=valueList;
        pthread_mutex_unlock( &ecatThreadMutex );
        return -1;
    }
    pthread_mutex_lock( &ecatThreadMutex );
    rt_printf(  "机器人 %d，零点标定ok. \n" , robID );
    funSetRobotValue( robID, updateHomeOffset, valueList );
    //重新赋值，防止线程不同步造成的误动
    for(int i=0;i<updateHomeOffset.size();i++)
    {
        QVector<int> motorAlias=robots.value(robID).motorAlias;
        for(int i = 0; i < motorAlias.size() ; i++ )
        {
            int nd = aliasMapNode.value(motorAlias.at(i));
            targetPos[nd] = encoderValue[nd] = realEncoderValue.at(nd)-updateHomeOffset.at(nd);
        }
    }
//    funSetRobotValue( robID, targetPos, 0 );//
//    funSetRobotValue( robID, encoderValue, 0 );//重新赋值
    tmp_updateHomeOffset.remove( robID );
    pthread_mutex_unlock( &ecatThreadMutex );
    return 0;
}

template <typename T>
void CoeEcatDevice::funSetRobotValue(int robID,
                                     QVector<T> &valBuff ,
                                     const QVector<T> &valNew )
{
    if( robID<0 )
    {
        //实际应用中应该不能全部机器人写同一个 vector数据的
        /* RobotInfo rob;
        foreach (rob, robots )
            {
            QVector<int> motorAlias= rob.motorAlias;
            for(int i = 0; i < motorAlias.size() ; i++ )
            {
                if(i>=aliasMapNode.size())
                    break;
                int nd = aliasMapNode.value(motorAlias.at(i));
                valBuff[nd] = valNew.at(i);
            }
        }*/
        qDebug()<<"goHomeRobot: robot ID can not be -1 !";
    }else{
        QVector<int> motorAlias=robots.value(robID).motorAlias;
        if( valNew.size() != motorAlias.size() ||  valBuff.size()<0 )
        {
            qDebug()<<"Error: 104 数据大小不一致! "<<valBuff <<valNew<< motorAlias ;
            return;
        }
        for(int i = 0; i < motorAlias.size() ; i++ )
        {
            int nd = aliasMapNode.value(motorAlias.at(i));
            valBuff[nd] = valNew.at(i);
        }
    }
}

template <typename T>
void CoeEcatDevice::funSetRobotValue(int robID,
                                     QVector<T> &valBuff ,
                                     int valNew )
{
    if( robID<0  )
    {
        RobotInfo rob;
        foreach (rob, robots)
        {
            if(   valBuff.size()<0 )
            {
                qDebug()<<"Error: 105 数据为空! "<<valBuff  ;
                return;
            }

            QVector<int> motorAlias= rob.motorAlias;
            for(int i = 0; i < motorAlias.size() ; i++ )
            {
                int nd = aliasMapNode.value( motorAlias.at(i) );
                valBuff[nd] = valNew;
            }
        }
    }else{
        if(   valBuff.size()<0 )
        {
            qDebug()<<"Error: 106 数据为空! "<<valBuff  ;
            return;
        }
        QVector<int> motorAlias=robots.value(robID).motorAlias;
        for(int i = 0; i < motorAlias.size() ; i++ )
        {
            int nd = aliasMapNode.value(motorAlias.at(i));
            valBuff[nd] = valNew;
        }
    }
}

template <typename T>
void CoeEcatDevice::funReSetRobotValue(int robID,
                                       QVector<T> &valBuff ,
                                       const QVector<T> &valNew )
{
    if( robID<0 )
    {
        if(   valBuff.size()<0 || valNew.size()<0 )
        {
            qDebug()<<"Error: 107 数据为空! "<<valBuff  ;
            return;
        }
        if( valBuff.size() != valNew.size() )
        {
            qDebug()<<"Error: 数据 长度不匹配! "<<valBuff<<valNew  ;
            return;
        }
        valBuff = valNew;
    }else{
        if(   valBuff.size()<0 || valNew.size()<0 )
        {
            qDebug()<<"Error: 108 数据为空! "<<valBuff  ;
            return;
        }
        QVector<int> motorAlias=robots.value(robID).motorAlias;
        for(int i = 0; i < motorAlias.size() ; i++ )
        {
            int nd = aliasMapNode.value(motorAlias.at(i));
            valBuff[nd] = valNew[nd];
        }
    }
}

template <typename T>
QVector<T> CoeEcatDevice::funGetRobotValue(int robID, const QVector<T> &valBuff )
{

    QVector<int> tmpVal;
    if( robID<0 )
    {
        /*
        RobotInfo rob;
        foreach (rob, robots )
            {
            QVector<int> motorAlias=robots.value(robID).motorAlias;
            tmpVal.resize( motorAlias.size() );
            for(int i = 0; i < motorAlias.size() ; i++ )
            {
                int nd = aliasMapNode.value(motorAlias.at(i));
                tmpVal[nd] = valBuff[nd];
            }
        }*/
        rt_printf( "goHomeRobot: robot ID can not be -1 !\n" );
    }else{
        QVector<int> motorAlias=robots.value(robID).motorAlias;
        tmpVal.resize( motorAlias.size() );
        for(int i = 0; i < motorAlias.size() ; i++ )
        {
            int nd = aliasMapNode.value(motorAlias.at(i));
            tmpVal[nd] = valBuff[nd];
        }
    }

    return tmpVal;
}

void CoeEcatDevice::homeParameterConfig(
        int robID,
        QVector <quint8> methodList,
        QVector <int> speedToSwitch,
        QVector <int> speedToZero,
        QVector <int> accList)
{
    if(!isInitOk){
        return;
    }
    funSetRobotValue(robID, homeMethod, methodList );
    funSetRobotValue(robID, homeSpeedToZ, speedToSwitch );
    funSetRobotValue(robID, homeSpeedToSwitch, speedToZero );
    funSetRobotValue(robID, homingAcc, accList );
}

//报警复位
void CoeEcatDevice::resetErrorRobot( int robID )
{
    //添加清除计时
   // tmax=0;
    max_time=0;
    min_time=cycle_ns;

#if D_IF_DEBUG
    TimeTool::getInstance()->resetAll();
#endif

    if( !isInitOk ){
        return ;
    }
    if( robID<0  )
    {
        //   qDebug()<<"resetErrorRobot: robot ID can not be -1 !";
        RobotInfo rob;
        foreach (rob, robots )
        {
            QVector<int> motorAlias= rob.motorAlias;
            for(int i = 0; i < motorAlias.size() ; i++ )
            {
                int nd=aliasMapNode.value(motorAlias.at(i));
                resetErrorNode(nd);
            }
        }
    }else{
        QVector<int> motorAlias= robots.value(robID).motorAlias;
        for(int i=0; i<motorAlias.size(); i++  )
        {
            int nd=aliasMapNode.value(motorAlias.at(i));
            resetErrorNode(nd);
        }
    }
}


// 函数功能：报警复位单个站点
bool CoeEcatDevice::resetErrorNode( int node )
{
    //    if( (0x80 == ( staWord[node]&0x80 ))
    //            ||(0x8 == ( staWord[node]&0x8 ))
    //            || sdoError !=0
    //            || aliasMatchError !=0
    //            || etherCatCommError !=0
    //            || slaveConfigError !=0
    //            || domainStateRrror !=0   )
    //    {
    sdoError =0;
    aliasMatchError =0;
    etherCatCommError =0;
    slaveConfigError =0;
    domainStateRrror =0;

    resetRequestFlag[node] = false;
    if( ctlWord[node] & 0x80 )
        ctlWord[node] &= (~0x80);
    else
        ctlWord[node] |= 0x80;
    //   return false;
    //  }

   // rt_printf("clear---error--------------");
    faultOccurred=false;
    warningState=false;
    return true;
}

void CoeEcatDevice::alarmSearch( int robID )
{
    /* if( robID<0  )
            {
            RobotInfo rob;
            foreach (rob, robots )
            {
                QVector<int> motorAlias= rob.motorAlias;
                for(int in = 0; in < motorAlias.size() ; in++ )
                {
                    int nd = aliasMapNode.value(motorAlias.at(in));
                    errorCode[nd]
                }
            }
        }else{
            QVector<int> motorAlias=robots.value(robID).motorAlias;
            for(int in = 0; in < motorAlias.size() ; in++ )
            {
                int nd = aliasMapNode.value(motorAlias.at(in));
                errorCode[nd]
            }
        }*/

}


void CoeEcatDevice::warningSearch( int robID )
{
    /* if( robID<0  )
            {
        RobotInfo rob;
        foreach (rob, robots )
            {
            QVector<int> motorAlias= rob.motorAlias;
            for(int in = 0; in < motorAlias.size() ; in++ )
            {
                int nd = aliasMapNode.value(motorAlias.at(in));
                errorCode[nd]
            }
        }
    }else{
        QVector<int> motorAlias=robots.value(robID).motorAlias;
        for(int in = 0; in < motorAlias.size() ; in++ )
        {
            int nd = aliasMapNode.value(motorAlias.at(in));
            errorCode[nd]
        }
    }*/
}

int CoeEcatDevice::getEcatDeviceConfig(int node )
{

    char returnBuf[100];
    char cmdBuf[5000];

    memset( cmdBuf, 0, 100*sizeof(char) );
    memset( returnBuf, 0, 5000*sizeof(char) );

    sprintf(cmdBuf, "ethercat cstruct -p %d -v" , node );

    getResultFromSystemCall( cmdBuf, returnBuf, 5000 );
    QString strReturn(returnBuf);

    if(strReturn.length()<1 )
    {
        return 2;
    }

    QString rx_config_str= "(entries[\\s\\S]+\\};)[\\s\\S]*(pdos[\\s\\S]+\\};)[\\s\\S]*(syncs[\\s\\S]+\\};)";

    QString rx_entry_str= "(\\{[ \\S]*\\})";
    QString rx_pdo_str= "(\\{[ \\S]*\\})";
    QString rx_sync_str= "(\\{[ \\S]*\\})";

    QString strEnties;
    QString strPdos;
    QString strSyncs;

    QRegExp rx;
    int pos=0;

    //获取所有配置
    rx.setPattern( rx_config_str );
    rx.setMinimal(true);
    if((pos = rx.indexIn(strReturn, pos)) != -1)
    {
        strEnties= rx.cap(1);
        strPdos= rx.cap(2);
        strSyncs= rx.cap(3);
    }else{
        qDebug()<< " failed ====";
        return false;
    }

    // qDebug()<< "==== strEnties"<< strEnties;
    //  qDebug()<< "==== strPdos"<< strPdos;
    //  qDebug()<< "==== strSyncs"<< strSyncs;

    //qDebug()<<"获取entry";
    int count =0;
    pos=0;
    rx.setPattern( rx_entry_str );
    rx.setMinimal(true);
    while( pos = rx.indexIn(strEnties, pos) != -1)
    {
        // QStringList sdsgasg=rx.capturedTexts();
        bool ok=false;
        QString tmpStr=rx.cap(0);
        if(tmpStr.isEmpty())
            break;

        int tpos=rx.pos(0);
        pos = tpos+tmpStr.length();
        tmpStr=tmpStr.mid(1,tmpStr.length()-2);

        QStringList valueList=tmpStr.split(",");
        if(valueList.size()==3 )
        {
            ecatDevs[node].objConfig.ec_entries.resize( count+1 );
            ecatDevs[node].objConfig.ec_entries[count].index=valueList.at(0).toInt(&ok,16) ;
            ecatDevs[node].objConfig.ec_entries[count].subindex=valueList.at(1).toInt(&ok,16) ;
            ecatDevs[node].objConfig.ec_entries[count].bit_length=valueList.at(2).toInt( ) ;
        }
        //  qDebug()<< tmpStr;
        count++;
    }
    if( ecatDevs[node].objConfig.ec_entries.size()<1 )
    {
        qDebug()<<"获取 entries 失败！";
        return 0;
    }

    //获取 pdo
    count=0;
    pos=0;
    rx.setPattern( rx_pdo_str );
    rx.setMinimal(true);
    while( pos = rx.indexIn(strPdos, pos) != -1 )
    {
        // QStringList sdsgasg=rx.capturedTexts();
        bool ok=false;
        QString tmpStr=rx.cap(0);
        if(tmpStr.isEmpty())
            break;

        int tpos=rx.pos(0);
        pos = tpos+tmpStr.length();
        tmpStr=tmpStr.mid(1,tmpStr.length()-2);

        QStringList valueList=tmpStr.split(",");
        if(valueList.size()==3 )
        {
            ecatDevs[node].objConfig.ec_pdos.resize( count+1 );
            ecatDevs[node].objConfig.ec_pdos[count].index=valueList.at(0).toInt(&ok,16) ;
            ecatDevs[node].objConfig.ec_pdos[count].n_entries=valueList.at(1).toInt( ) ;

            QString tmpStr2=valueList.at(2);
            tmpStr2= tmpStr2.mid(tmpStr2.indexOf("+")+sizeof('+'));
            ecatDevs[node].objConfig.ec_pdos[count].entries= &ecatDevs[node].objConfig.ec_entries[  tmpStr2.toInt( )  ];

            // qDebug()<< tmpStr<< tmpStr2;
        }
        //    qDebug()<< valueList;
        count++;
    }
    if( ecatDevs[node].objConfig.ec_pdos.size()<1 )
    {
        qDebug()<<"获取 pdos 失败！";
        return 0;
    }

    //获取 sync
    count=0;
    pos=0;
    rx.setPattern( rx_sync_str );
    while( pos = rx.indexIn(strSyncs, pos) != -1)
    {
        // QStringList sdsgasg=rx.capturedTexts();
        bool ok=false;
        QString tmpStr=rx.cap(0);
        if(tmpStr.isEmpty())
            break;

        int tpos=rx.pos(0);
        pos = tpos+tmpStr.length();
        tmpStr=tmpStr.mid(1,tmpStr.length()-2);

        qDebug()<< tmpStr;
        QStringList valueList=tmpStr.split(",");
        if(valueList.size()==5 )
        {
            ecatDevs[node].objConfig.ec_syncs.resize( count+1 );
            ecatDevs[node].objConfig.ec_syncs[count].index=valueList.at(0).toInt(&ok,16) ;
            QString tmpStr2=valueList.at(1);
            if( tmpStr2.indexOf("EC_DIR_OUTPUT")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].dir= EC_DIR_OUTPUT ;
            }else if( tmpStr2.indexOf("EC_DIR_INPUT")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].dir= EC_DIR_INPUT ;
            }else if( tmpStr2.indexOf("EC_DIR_INVALID")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].dir= EC_DIR_INVALID ;
            }else if( tmpStr2.indexOf("EC_DIR_COUNT")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].dir= EC_DIR_COUNT ;
            }else{
                qDebug()<<"errrrrrrrrrrrrrrrrrr"<<node<<tmpStr2;
                return 0;
            }
            ecatDevs[node].objConfig.ec_syncs[count].n_pdos = valueList.at(2).toInt( ) ; ;

            QString tmpStr3=valueList.at(3);
            tmpStr3= tmpStr3.mid(tmpStr3.indexOf("+")+sizeof('+'));
            ecatDevs[node].objConfig.ec_syncs[count].pdos= &ecatDevs[node].objConfig.ec_pdos[(tmpStr3.toInt( ))];

            QString tmpStr4=valueList.at(4);
            if( tmpStr4.indexOf("EC_WD_DISABLE")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].watchdog_mode= EC_WD_DISABLE ;
            }else if( tmpStr4.indexOf("EC_WD_ENABLE")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].watchdog_mode= EC_WD_ENABLE ;
            }else if( tmpStr4.indexOf("EC_WD_DEFAULT")>=0  )
            {
                ecatDevs[node].objConfig.ec_syncs[count].watchdog_mode= EC_WD_DEFAULT ;
            }else {
                qDebug()<<"errrrrrrrrrrrrrrrrrr"<<node<< tmpStr4;
                return 0;
            }

            if( ecatDevs[node].objConfig.ec_syncs[count].dir== EC_DIR_OUTPUT  )
                ecatDevs[node].objConfig.ec_syncs[count].watchdog_mode;

            // qDebug()<< valueList;
            count++;
        }
    }
    if( ecatDevs[node].objConfig.ec_syncs.size()<1 )
    {
        qDebug()<<"获取 sync 失败！";
        return 0;
    }

    qDebug()<<"get ethercat slave config success！"<<node;

    //  MessageLog::getInstance()->addMessage( ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEcatDevice",  21, -1 , node);
    addMsg(ENUM_MSG_ERROR, "CoeEcatDevice", "CoeEthercat", 21, -1, node );
    return 1;
}

int CoeEcatDevice::clearMessage( Message msg )
{
    if( !isInitOk ){
        return 0;
    }
    ;//判断轴号，消息类型
    if( (msg.messageCode >= 22 && msg.messageCode <= 29)
            || msg.messageCode==30
            || msg.messageCode==31
            || msg.messageCode==34 )
    {
        resetErrorRobot( msg.robotId  );
    }
    else
    {
        return -1;
    }

    return 0;
}

#include "ethercaterror.h"
void CoeEcatDevice::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
    tmpMsg.time = t1.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {
    case 1:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("控制电源不足电压保护");

    }
        break;
    case 2:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("过电压保护");

    }
        break;
    case 3:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +   " ," +QObject::tr("主电源不足电压保护，PN键");

    }
        break;
    case 4:
    {

    }
        break;
    case 5:
    {

    }
        break;
    case 6:
    {
        infomationStr =  QObject::tr("总线配置文件所指定的设备没有找到!")  + QString::number(parameter1);

    }
        break;
    case 7:
    {
        infomationStr =  QObject::tr("从厂商配置文件读取配置信息失败：无法打开文件！");

    }
        break;
    case 8:
    {
        infomationStr =  QObject::tr("从厂商配置文件读取配置信息失败，厂商ID错误: : node=") + QString::number(parameter1);

    }
        break;
    case 9:
    {
        infomationStr =  QObject::tr("从厂商配置文件读取配置信息失败：无此节点信息: ") + QString::number(parameter1);

    }
        break;
    case 10:
    {
        infomationStr =  QObject::tr("读取机器人配置信息出错 !");

    }
        break;
    case 11:
    {
        infomationStr =  QObject::tr("读取ethercat总线配置信息出错!");

    }
        break;
    case 12:
    {
        infomationStr =  QObject::tr("请求主站无响应！");

    }
        break;
    case 13:
    {
        infomationStr =  QObject::tr("ethercat配置过程：域数据寄存器配置失败！");

    }
        break;
    case 14:
    {
        infomationStr =  QObject::tr("从站别名检测错误！") +QString::number(parameter1);
    }
        break;
    case 15:
    {
        infomationStr =  QObject::tr("初始化设备失败！");

    }
        break;
    case 16:
    {
        infomationStr =  QObject::tr("激活主站失败！");

    }
        break;
    case 17:
    {
        infomationStr =  QObject::tr("无效的总线设备, 别名：")
                + QString::number(parameter1);

    }
        break;
    case 18:
    {
        infomationStr =  QObject::tr("无效的机器人设备, 别名:");

    }
        break;
    case 19:
    {
        infomationStr= QObject::tr("从站配置失败：PDO映射错误！站号（按位算）:")
                + QString::number(parameter1);

    }
        break;
    case 20:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴")
                + QString::number(parameter1+1)
                +  " ," +QObject::tr("回零步骤操作超时 1 !");
    }
        break;
    case 21:
    {
        infomationStr =  QObject::tr("获取ethercat从站配置成功, 站号:") + QString::number(parameter1);

    }
        break;
    case 22:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("主电源缺相！");

    }
        break;
    case 23:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("电池电量不足！");

    }
        break;
    case 24:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("扭矩指令过大！");

    }
        break;
    case 25:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("速度指令过大！");

    }
        break;
    case 26:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr + " ," + QObject::tr("过载警告!");

    }
        break;
    case 27:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("选定容量警告!");

    }
        break;
    case 28:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("低电压警告！");

    }
        break;
    case 29:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," +QObject::tr("EMG警告！");
    }
        break;
    case 30:
    {
        infomationStr = QObject::tr("域数据读写错误！");

    }
    case 31:
    {
        infomationStr =  QObject::tr("ethercat通信WKC异常!")
                +  " ,"+QString::number(parameter1+1)
                + ", "+ QString::number(parameter2)
                + ", "+QString::number(parameter3);

    }
        break;
    case 32:
    {
        infomationStr = QObject::tr("控制器运行发生错误，多次定时间隔小于20us！");

    }
        break;
    case 33:
    {
        infomationStr =  QObject::tr("总线设备存在重复别名:")
                + QString::number(parameter1+1)
                + QString::number(parameter2);
    }
        break;
    case 34:
    {
        infomationStr =  QObject::tr("伺服报警:机器人:")  + QString::number(robotId)
                + QObject::tr(", 轴:") + QString::number(parameter1+1)
                + QObject::tr(", 错误码:")  + QString::number(parameter2)+" ,"
                + EthercatError::geterror( parameter3, parameter2 );
    }
        break;
    case 35:
    {
        infomationStr =  QObject::tr("ethercat 驱动未启动!");
    }
        break;
    case 36:
    {
        infomationStr=  QObject::tr("ethercat 设备启动失败, 请尝试重启！设备节点:")+ QString::number(parameter1);
    }
        break;
    case 37:
    {
        infomationStr =  QObject::tr("ethercat 设备检测失败: 数目错误: ") + QString::number(parameter1);
    }
        break;
    case 38:
    {
        infomationStr =  QObject::tr("ethercat 设备检测: 数目为零错误!");

    }
        break;
    case 39:
    {
        infomationStr =  QObject::tr("ethercat 设备长时间没有响应, 请检查网卡或网络状态！");

    }
        break;
    case 40:
    {
        infomationStr =  QObject::tr("零点标定数据更新失败，ethercat未完成初始化！")
                + QString::number(robotId);
    }
        break;
    case 41:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",轴");
        infomationStr = infomationStr + QString::number(parameter1+1);
        infomationStr = infomationStr +  " ," + QObject::tr("伺服发生警告,警告码 :")+ QString::number(parameter2)
                + EthercatError::getwarn( parameter3, parameter2 );
    }
        break;
    }

    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}



